...
Call the LandingTakeoffManager to calculate the waypoints to follow to get to the takeoff height. Feed that data into the waypoint manager.
Code Prototype:
For calculating waypoint targets after the ground height sensor has been checked to see if the drone has landed.
| Code Block | ||
|---|---|---|
| ||
_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 = getAltitudeTarget(curAltitude, groundHeight);
return desiredWaypoint;
}
double LandingTakeoffManager::getAltitudeTarget(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;
} |
Landing Takeoff Manager:
Use waypoints as targets that the drone attempts to meet at a set frequency. As drone approaches target, shorten the distance to the next waypoint to slow down drone ascent.
...