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

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)

Code Prototype:

For calculating waypoint targets during Takeoff.

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::createTakeoffWaypoint(const SFOutput_t & input)
{
    // Set starting Lat/long on first loop
    if (startLat == -1 and startLong == -1) {
        startLat = input.latitude;
        startLong = input.longitude;
    }
    // Save the starting ground height reading for landing.
    if (groundHeight == -1) {
        groundHeight = input.altitude;
    }
    _PathData desiredWaypoint;

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

    double curAltitude = input.altitude;

    desiredWaypoint.velocity = getTakeoffVelocityTarget(curAltitude, groundHeight);
    desiredWaypoint.altitude = curAltitude + desiredWaypoint.velocity * (1.0 / CONTROLLER_FREQ);

    return desiredWaypoint;
}

double LandingTakeoffManager::getTakeoffVelocityTarget(double curAltitude)
{
    double velocityTarget;
    velocityTarget = MAX_VELOCITY * exp(-1.0 * (((curAltitude - groundHeight - (TAKEOFF_TARGET / 2))^2) / RANGE_CONSTANT))
    return velocityTarget;
}

Math Screenshots in case Desmos link fails

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

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;
}