Page Comparison

_PathData LandingTakeoffManager::createLandingWaypointcreateTakeoffWaypoint(const SFOutput_t & input)
{
    // Use current Lat/long if origin not known
    if (landingLat == -1 and landingLong == -1) {
        landingLat = input.latitude;
        landingLong = input.longitude;
    }
    _PathData desiredWaypoint;

    desiredWaypoint.latitude = landingLat;
    desiredWaypoint.longitude = landingLong;
    desiredWaypoint.waypointType = LANDING_WAYPOINT;

    int curAltitude = input.altitude;

    desiredWaypoint.altitude = getAltitudeTargetgetTakeoffAltitudeTarget(curAltitude, groundHeight);

    return desiredWaypoint;
}

double LandingTakeoffManager::getAltitudeTargetgetTakeoffAltitudeTarget(double curAltitude, double targetAltitude)
{
    double waypointTarget;
    double droneHeighttargetDistance = (curAltitudetargetAltitude - targetAltitudecurAltitude);
    if (droneHeighttargetDistance < 1.0) {
        // TouchdownFinal Takeoff speed of 0.1 m/s, set as waypoint targets at PM Freq
        waypointTarget = curAltitude -+ (0.1 / PM_FREQ);
    } else {
        // Scale descentascent velocity based on 0.2 * heighttargetDistance (Eg. 0.4 m/s at 2m away)
        // Use altitude waypoint targets geing calculated at PM Freq
        waypointTarget = droneHeight * (01.2 / PM_FREQ);
    }
    return waypointTarget;
}