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()); } }