void autopilot_static_periodic(void)
{
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight() && autopilot.mode == AP_MODE_NAV) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
if (dist2_to_home > failsafe_mode_dist2) {
autopilot_static_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_static_set_mode(AP_MODE_HOME);
}
}
}
if (autopilot.mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
} else {
// otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot.mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight()) {
autopilot_static_set_mode(AP_MODE_KILL);
}
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot.ground_detected) {
autopilot_static_set_mode(AP_MODE_KILL);
}
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight()) {
autopilot.ground_detected = false;
autopilot.detect_ground_once = false;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot.mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
} else {
guidance_v_run(autopilot_in_flight());
guidance_h_run(autopilot_in_flight());
SetRotorcraftCommands(stabilization_cmd, autopilot.in_flight, autopilot.motors_on);
}
}
/** AP mode setting handler
*
* Checks RC status before calling autopilot_static_set_mode function
*/
void autopilot_static_SetModeHandler(float mode)
{
if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
// safety modes are always accessible via settings
autopilot_static_set_mode(mode);
} else {
if (radio_control.status != RC_OK &&
(mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
// without RC, only nav-like modes are accessible
autopilot_static_set_mode(mode);
}
}
// with RC, other modes can only be changed from the RC
}
/** set autopilot mode
*/
void autopilot_set_mode(uint8_t new_autopilot_mode)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_set_mode(new_autopilot_mode);
#else
autopilot_static_set_mode(new_autopilot_mode);
#endif
}
void autopilot_static_init(void)
{
/* Mode is finally set by autopilot_static_set_mode if MODE_STARTUP is not KILL.
* For autopilot_static_set_mode to do anything, the requested mode needs to differ
* from previous mode, so we set it to a safe KILL first.
*/
autopilot.mode = AP_MODE_KILL;
/* set startup mode, propagates through to guidance h/v */
autopilot_static_set_mode(MODE_STARTUP);
/* init arming */
autopilot_arming_init();
}
void autopilot_static_on_rc_frame(void)
{
if (kill_switch_is_on()) {
autopilot_static_set_mode(AP_MODE_KILL);
} else {
#ifdef RADIO_AUTO_MODE
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
uint8_t new_autopilot_mode = ap_mode_of_two_switches();
#else
#ifdef RADIO_MODE_2x3
uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
#else
uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
#endif
#endif
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
/* always allow to switch to manual */
if (new_autopilot_mode == MODE_MANUAL) {
autopilot_static_set_mode(new_autopilot_mode);
}
/* if in HOME mode, don't allow switching to non-manual modes */
else if ((autopilot.mode != AP_MODE_HOME)
#if UNLOCKED_HOME_MODE
/* Allowed to leave home mode when UNLOCKED_HOME_MODE */
|| !too_far_from_home
#endif
) {
autopilot_static_set_mode(new_autopilot_mode);
}
}
}
/* an arming sequence is used to start/stop motors.
* only allow arming if ahrs is aligned
*/
if (ap_ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
autopilot.kill_throttle = ! autopilot.motors_on;
} else {
autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED;
}
/* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
if (autopilot.mode != AP_MODE_FAILSAFE && autopilot.mode != AP_MODE_HOME) {
/* if there are some commands that should always be set from RC, do it */
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
#ifdef SetCommandsFromRC
if (autopilot.mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight());
}
}
