void VtolFlyController::UpdateAutoPilot() { if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { mManualThrust = true; } uint8_t result = RunAutoPilot(); if (result) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); } else { pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); // If rtbl, detect arrival at the endpoint and then triggers a change // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c // can't manage this. And pathplanner whilst similar does not manage this as it is not a // waypoint traversal and is not aware of flight modes other than path plan. if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) { if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) { if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) { if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) { plan_setup_land(); } } } } }
static void plan_setup_land_from_velocityroam() { plan_setup_land(); FlightStatusAssistedControlStateOptions assistedControlFlightMode; assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; FlightStatusAssistedControlStateSet(&assistedControlFlightMode); }
/** * @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; } }
void plan_run_AutoTakeoff() { StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState; switch (autotakeoffState) { case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (!flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE: { ManualControlCommandData cmd; ManualControlCommandGet(&cmd); if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE: { ManualControlCommandData cmd; ManualControlCommandGet(&cmd); if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT; plan_setup_land(); } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (!flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD: // nothing to do. land has been requested. stay here for forever until mode change. default: break; } if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT && autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) { if (priorState != autotakeoffState) { PathDesiredData pathDesired; plan_setup_AutoTakeoff_helper(&pathDesired); PathDesiredSet(&pathDesired); } } }