...
Code Block | ||||
---|---|---|---|---|
| ||||
Variables owned by PM: const double TAKEOFF_TARGET = 4.0; const double LANDING_SPEED = 0.2; const double MAX_VELOCITYSPEED = 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; } |
...