Takeoff

Considerations:

Spin up to takeoff speed (Reduce current spikes/easier on ESC & motors)
Automated, but can be pilot overridden
Transition to hover at set height (4m) if no pilot input
If comms loss during takeoff transition to hover in place and if timeout passes transition to landing?
Uneven takeoff surface

The Plan:

MVP first:

At signal (Not when powered on) can lift off and ascend using a state in path manager that passes targets to attitude manager
At another signal transitions out of takeoff state

Later steps:

After ascent, transitions to a hover target
Update the target ground height with a sensor (Would be nice to stop assuming earth is flat)
Use Passby control to “spin up” motors to 20% throttle before takeoff, this reduces current spikes and increases motor longevity.
Use optical Flow sensor to keep waypoint targets perfectly vertical
On Comms Loss transition to Landing
Handling of fatal failures

Path Manager Component:

Call the LandingTakeoffManager to calculate the waypoints to follow to get to the takeoff height. Feed that data into the attitude manager.

Velocity Waypoint Targets

Calculate ascent speed using Gaussian Profile:

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

To ascend to a set height (4m)

Math Screenshots in case Desmos link fails

Velocity-less design (Ideally not in use for 2023)

Uses waypoints which cannot match the smoothness of a velocity profile.

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

Code Prototype:

For calculating waypoint targets during Takeoff.

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

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

    int curAltitude = input.altitude;

    desiredWaypoint.altitude = getTakeoffAltitudeTarget(curAltitude, groundHeight);

    return desiredWaypoint;
}

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