...
Code Block | ||||
---|---|---|---|---|
| ||||
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;
} |
...