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);
}
}
void autopilot_periodic(void) {
if (autopilot_in_flight) {
if (too_far_from_home) {
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 {
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) {
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);
}
}
/** \fn void navigation_task( void )
* \brief Compute desired_course
*/
static void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (GpsTimeoutError) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
gps_lost = TRUE;
}
} else /* GPS is ok */ if (gps_lost) {
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
nav_without_gps();
else
nav_periodic_task();
#ifdef TCAS
CallTCAS();
#endif
#ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(DefaultChannel);
#endif
SEND_CAM(DefaultChannel);
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
v_ctl_altitude_loop();
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = FALSE;
#endif
if (lateral_mode >=LATERAL_MODE_COURSE)
h_ctl_course_loop(); /* aka compute nav_desired_roll */
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
v_ctl_climb_loop();
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
v_ctl_throttle_setpoint = nav_throttle_setpoint;
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
h_ctl_pitch_setpoint = nav_pitch;
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!estimator_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours)
}
/**
* Compute desired_course
*/
void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (GpsTimeoutError) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
gps_lost = TRUE;
}
} else if (gps_lost) { /* GPS is ok */
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
nav_without_gps();
else
nav_periodic_task();
#ifdef TCAS
CallTCAS();
#endif
#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(DefaultChannel, DefaultDevice);
#endif
SEND_CAM(DefaultChannel, DefaultDevice);
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
v_ctl_altitude_loop();
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = FALSE;
#endif
if (lateral_mode >=LATERAL_MODE_COURSE)
h_ctl_course_loop(); /* aka compute nav_desired_roll */
// climb_loop(); //4Hz
}
energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours)
}