Landing

Considerations:

Transition to landing from any state
Automated with pilot override
Initiate gaussian profile for descent velocity
Landing Detection
Emergency state (Can be automatically transitioned to on coms loss)
Uneven landing surface
- Eg, Only half drone experiencing ground effect, try not to crash
Spin-down after landing is detected

The Plan:

MVP first:

At signal (Not when powered on) can descend using a state in path manager that passes targets to attitude manager
At another signal transitions out of landing state
When altitude has been constant for a few seconds and below threshold then transition out of landing.

Later steps:

Ensure drone is in a hover before descending
After descent, transitions to a standby or disarmed state
Update the target ground height with a sensor (Would be nice to stop assuming earth is flat)
Use Passby control to “spin down” motors to 20% throttle after landing (in standby state), increases motor longevity and can prevent motor cogging.
Use optical Flow sensor to keep waypoint targets perfectly vertical
Add a better landing detection system (possibly with optical flow sensor)

Path Manager Component:

Call the LandingTakeoffManager to calculate the waypoints to follow to get to the ground. Feed that data into the Attitude Manager.

Landing Detection:

This may require a sensor (optical flow) or something as simple as a limit switch attached to an antenna pointed at the ground. When landing is detected, the drone will either directly transition to the next mode or move to spin down/cool off the motors

Velocity-based Landing Takeoff Manager:

Use Gaussian Profile equation to generate a velocity:

https://www.desmos.com/calculator/tjtx8ogtno

Code Prototype:

Variables owned by PM:
  const double TAKEOFF_TARGET = 4.0;
  const double LANDING_SPEED = 0.2;
  const double MAX_VELOCITY = 1.0;
  const double RANGE_CONSTANT = 2.48533973824; // See Desmos Graph for this calculation
  double groundHeight = -1; // Set by takeoff and used by landing (Ideally later updated by sensors)
  double startLat = -1; // Set at start of takeoff and landing reset by flight, hover, or boot
  double startLong = -1; // Set at start of takeoff and landing reset by flight, hover, or boot
  
Variable that should be defined somewhere that this will need:
  const int CONTROLLER_FREQ = ;

_PathData LandingTakeoffManager::createLandingWaypoint(const SFOutput_t & input)
{
    // Set starting Lat/long on first loop
    if (startLat == -1 and startLong == -1) {
        startLat = input.latitude;
        startLong = input.longitude;
    }
    _PathData desiredWaypoint;

    desiredWaypoint.latitude = startLat;
    desiredWaypoint.longitude = startLong;
    desiredWaypoint.waypointType = LANDING_WAYPOINT;

    double curAltitude = input.altitude;

    desiredWaypoint.velocity = getLandingVelocityTarget(curAltitude);
    desiredWaypoint.altitude = curAltitude - desiredWaypoint.velocity * (1.0 / CONTROLLER_FREQ);

    return desiredWaypoint;
}

double LandingTakeoffManager::getLandingSpeedTarget(double curAltitude)
{
    double speedTarget;
    speedTarget = MAX_SPEED * exp(-1.0 * (((curAltitude - groundHeight - (TAKEOFF_TARGET / 2))^2) / RANGE_CONSTANT))
    return speedTarget;
}

Waypoint-based 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.

The profile to be used can be seen below:

https://www.desmos.com/calculator/kbamddthi9

Note that for low altitude landings the done will not use the higher descent velocity part of the profile right at the start and instead will be given the slower descent speed so the drone does not accelerate into the ground.

For calculating waypoint targets after the ground height sensor has been checked to see if the drone has landed.

_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 = getLandingAltitudeTarget(curAltitude, groundHeight);

    return desiredWaypoint;
}

double LandingTakeoffManager::getLandingAltitudeTarget(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;
}