void autopilot_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_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_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_set_mode(AP_MODE_KILL);
}
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected) {
autopilot_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);
}
}
/** \brief Home mode navigation (circle around HOME) */
void nav_home(void) {
NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS);
/** Nominal speed */
nav_pitch = 0.;
v_ctl_mode = V_CTL_MODE_AUTO_ALT;
nav_altitude = ground_alt+HOME_MODE_HEIGHT;
compute_dist2_to_home();
dist2_to_wp = dist2_to_home;
nav_set_altitude();
}
/**
* \brief Navigation main: call to the code generated from the XML flight
* plan
*/
void nav_periodic_task(void) {
nav_survey_active = FALSE;
compute_dist2_to_home();
dist2_to_wp = 0.;
auto_nav(); /* From flight_plan.h */
h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
#ifdef AGR_CLIMB
if ( v_ctl_mode == V_CTL_MODE_AUTO_CLIMB)
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
#endif
nav_set_altitude();
}
