Page Comparison

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 hoverboot
  double startLong = -1; // Set at start of takeoff and landing reset by flight, hover, or hoverboot
  
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;
}