void plan_setup_AutoTakeoff()
{
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
// We only allow takeoff if the state transition of disarmed to armed occurs
// whilst in the autotake flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
StabilizationDesiredData stabiDesired;
StabilizationDesiredGet(&stabiDesired);
// Are we inflight?
if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
// ok assume already in flight and just enter position hold
// if we are not actually inflight this will just be a violent autotakeoff
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
plan_setup_positionHold();
} else {
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
// Note that if this mode was invoked unintentionally whilst in flight, effectively
// all inputs get ignored and the vtol continues to fly to its previous
// stabi command.
}
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
static void plan_setup_PositionVario()
{
vario_hold = true;
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
AttitudeStateYawGet(&vario_course);
plan_setup_positionHold();
}
void plan_setup_land()
{
float descendspeed;
plan_setup_positionHold();
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
static void plan_setup_PositionVario()
{
vario_hold = true;
plan_setup_positionHold();
}
void plan_setup_land()
{
plan_setup_positionHold();
}
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit)
{
if (newinit) {
plan_initialize();
}
FlightStatusFlightModeOptions flightMode;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusFlightModeAssistOptions flightModeAssist;
FlightStatusFlightModeGet(&flightMode);
FlightStatusFlightModeAssistGet(&flightModeAssist);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (newinit) {
// After not being in this mode for a while init at current height
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
plan_setup_returnToBase();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
plan_setup_assistedcontrol();
} else {
plan_setup_positionHold();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
plan_setup_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
plan_setup_VelocityRoam();
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_setup_HomeLeash();
break;
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
plan_setup_AbsolutePosition();
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_setup_land();
} else {
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_setup_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_setup_AutoCruise();
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
plan_setup_assistedcontrol();
}
break;
default:
plan_setup_positionHold();
break;
}
}
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
plan_run_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
plan_run_VelocityRoam();
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_run_HomeLeash();
break;
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
plan_run_AbsolutePosition();
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_run_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_run_AutoCruise();
break;
default:
break;
}
}