Page Comparison

_PathData LandingTakeoffManager::createLandingWaypoint(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 = getAltitudeTargetgetLandingAltitudeTarget(curAltitude, groundHeight);

    return desiredWaypoint;
}

double LandingTakeoffManager::getAltitudeTargetgetLandingAltitudeTarget(double curAltitude, double targetAltitude)
{
    double waypointTarget;
    double droneHeight = (curAltitude - targetAltitude);
    if (droneHeight < 1.0) {
        // Touchdown speed of 0.1 m/s, set as waypoint targets at PM Freq
        waypointTarget = curAltitude - (0.1 / PM_FREQ);
    } else {
        // Scale descent velocity based on 0.2 * height (Eg. 0.4 m/s at 2m)
        // Use altitude waypoint targets geing calculated at PM Freq
        waypointTarget = droneHeight * (0.2 / PM_FREQ);
    }
    return waypointTarget;
}