void attitude_loop( void ) { #if USE_INFRARED ahrs_update_infrared(); #endif /* USE_INFRARED */ if (pprz_mode >= PPRZ_MODE_AUTO2) { if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) { v_ctl_throttle_setpoint = nav_throttle_setpoint; v_ctl_pitch_setpoint = nav_pitch; } else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) { v_ctl_climb_loop(); } #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 = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control 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; } h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint; ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK link_mcu_send(); #elif defined INTER_MCU && defined SINGLE_MCU /**Directly set the flag indicating to FBW that shared buffer is available*/ inter_mcu_received_ap = TRUE; #endif }
/** \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) }