static inline void attitude_loop( void ) {
#ifdef USE_GYRO
gyro_update();
#endif
#ifdef USE_INFRARED
infrared_update();
estimator_update_state_infrared();
#endif /* USE_INFRARED */
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
}
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
}
void periodic_task_ap( void ) {
static uint8_t _20Hz = 0;
static uint8_t _10Hz = 0;
static uint8_t _4Hz = 0;
static uint8_t _1Hz = 0;
_20Hz++;
if (_20Hz>=3) _20Hz=0;
_10Hz++;
if (_10Hz>=6) _10Hz=0;
_4Hz++;
if (_4Hz>=15) _4Hz=0;
_1Hz++;
if (_1Hz>=60) _1Hz=0;
reporting_task();
if (!_1Hz) {
if (estimator_flight_time) estimator_flight_time++;
#if defined DATALINK || defined SITL
datalink_time++;
#endif
static uint8_t t = 0;
if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
kill_throttle |= (t >= LOW_BATTERY_DELAY);
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
}
switch(_4Hz) {
case 0:
#ifdef SITL
#ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION();
#endif
#endif
estimator_propagate_state();
#ifdef EXTRA_DOWNLINK_DEVICE
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
#endif
navigation_task();
break;
case 1:
if (!estimator_flight_time &&
estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
estimator_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
default:
break;
}
break;
#ifdef USE_GPIO
case 3:
GpioUpdate1();
break;
#endif
/* default: */
}
#ifndef CONTROL_RATE
#define CONTROL_RATE 20
#endif
#if CONTROL_RATE != 60 && CONTROL_RATE != 20
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif
#ifdef USE_ANALOG_IMU
if (!_20Hz) {
imu_periodic();
}
#endif // USE_ANALOG_IMU
#if CONTROL_RATE == 20
if (!_20Hz)
#endif
{
#ifdef USE_GYRO
gyro_update();
#endif
#ifdef USE_INFRARED
infrared_update();
estimator_update_state_infrared();
#endif /* USE_INFRARED */
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
}
modules_periodic_task();
}
