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