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