/**
* @brief execute land
*/
void plan_run_land()
{
PathDesiredEndData pathDesiredEnd;
PathDesiredEndGet(&pathDesiredEnd);
PositionStateDownGet(&pathDesiredEnd.Down);
pathDesiredEnd.Down += 5;
PathDesiredEndSet(&pathDesiredEnd);
}
void VtolLandFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
mLandData->flLowAltitude = false;
} else {
mLandData->flLowAltitude = true;
}
}
/**
* @brief execute land
*/
void plan_run_land()
{
float downPos, descendspeed;
PathDesiredEndData pathDesiredEnd;
PositionStateDownGet(&downPos); // current down position
PathDesiredEndGet(&pathDesiredEnd); // desired position
PathDesiredEndingVelocityGet(&descendspeed);
// desired position is updated to match the desired descend speed but don't run ahead
// too far if the current position can't keep up. This normaly means we have landed.
if (pathDesiredEnd.Down - downPos < 10) {
pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
}
PathDesiredEndSet(&pathDesiredEnd);
}
/**
* @brief setup pathplanner/pathfollower for return to base
*/
void plan_setup_returnToBase()
{
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
float positionStateDown;
PositionStateDownGet(&positionStateDown);
PathDesiredData pathDesired;
// re-initialise in setup stage
memset(&pathDesired, 0, sizeof(PathDesiredData));
TakeOffLocationData takeoffLocation;
TakeOffLocationGet(&takeoffLocation);
// TODO: right now VTOLPF does fly straight to destination altitude.
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
float destDown;
FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East;
pathDesired.End.Down = destDown;
pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = destDown;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
/**
* @brief setup pathplanner/pathfollower for return to base
*/
void plan_setup_returnToBase()
{
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
float positionStateDown;
PositionStateDownGet(&positionStateDown);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
TakeOffLocationData takeoffLocation;
TakeOffLocationGet(&takeoffLocation);
// TODO: right now VTOLPF does fly straight to destination altitude.
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
float destDown;
FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East;
pathDesired.End.Down = destDown;
pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = destDown;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
