void handle_periodic_tasks_ap(void) {
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();//imu数据读取
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();//估算期望的航线，故障保护
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();//姿态循环
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();//没有该函数说明
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();//监听
if (sys_time_check_and_ack_timer(telemetry_tid))
{
reporting_task();//汇报
LED_PERIODIC();
}
}
value sim_periodic_task(value unit) {
sensors_task();
attitude_loop();
reporting_task();
modules_periodic_task();
periodic_task_fbw();
event_task_ap();
event_task_fbw();
return unit;
}
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
#ifndef SINGLE_MCU
#if defined USE_I2C0 || defined USE_I2C1 || defined USE_I2C2
i2c_event();
#endif
#endif
#if USE_AHRS && USE_IMU
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif
#if USE_INS
InsEvent(NULL);
#endif
#if USE_GPS
GpsEvent(on_gps_solution);
#endif /* USE_GPS */
#if USE_BAROMETER
BaroEvent(on_baro_abs_event, on_baro_dif_event);
#endif
DatalinkEvent();
#ifdef MCU_SPI_LINK
link_mcu_event_task();
#endif
if (inter_mcu_received_fbw) {
/* receive radio control task from fbw */
inter_mcu_received_fbw = FALSE;
telecommand_task();
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
attitude_loop();
//LED_TOGGLE(3);
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
value sim_periodic_task(value unit)
{
sensors_task();
#if USE_GENERATED_AUTOPILOT
autopilot_periodic();
#else
attitude_loop();
#endif
reporting_task();
modules_periodic_task();
periodic_task_fbw();
electrical_periodic();
event_task_ap();
event_task_fbw();
return unit;
}
void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid)) {
sensors_task();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
#if USE_GENERATED_AUTOPILOT
if (sys_time_check_and_ack_timer(attitude_tid)) {
autopilot_periodic();
}
#else
// static autopilot
if (sys_time_check_and_ack_timer(navigation_tid)) {
navigation_task();
}
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid)) {
attitude_loop();
}
#endif
#endif
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
}
if (sys_time_check_and_ack_timer(monitor_tid)) {
monitor_task();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
#ifdef USE_INFRARED
infrared_event();
#endif
#ifdef USE_AHRS
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif // USE_AHRS
#ifdef USE_GPS
GpsEvent(on_gps_solution);
#endif /** USE_GPS */
DatalinkEvent();
#ifdef MCU_SPI_LINK
link_mcu_event_task();
#endif
if (inter_mcu_received_fbw) {
/* receive radio control task from fbw */
inter_mcu_received_fbw = FALSE;
telecommand_task();
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
attitude_loop();
//LED_TOGGLE(3);
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
#ifdef USE_INFRARED
infrared_event();
#endif
#ifdef USE_AHRS
ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
#endif // USE_AHRS
#ifdef USE_GPS
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */
if (GpsBuffer()) {
ReadGpsBuffer();
}
#endif
if (gps_msg_received) {
/* parse and use GPS messages */
#ifdef GPS_CONFIGURE
if (gps_configuring)
gps_configure();
else
#endif
parse_gps_msg();
gps_msg_received = FALSE;
if (gps_pos_available) {
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_downlink();
#ifdef GPS_TRIGGERED_FUNCTION
#ifndef SITL
GPS_TRIGGERED_FUNCTION();
#endif
#endif
gps_pos_available = FALSE;
}
}
#endif /** USE_GPS */
DatalinkEvent();
#ifdef MCU_SPI_LINK
link_mcu_event_task();
#endif
if (inter_mcu_received_fbw) {
/* receive radio control task from fbw */
inter_mcu_received_fbw = FALSE;
telecommand_task();
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
attitude_loop();
//LED_TOGGLE(3);
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
void periodic_task_ap( void ) {
static uint8_t _60Hz = 0;
static uint8_t _20Hz = 0;
static uint8_t _10Hz = 0;
static uint8_t _4Hz = 0;
static uint8_t _1Hz = 0;
#ifdef USE_IMU
// Run at PERIODIC_FREQUENCY (60Hz if not defined)
imu_periodic();
#endif // USE_IMU
#define _check_periodic_freq_ PERIODIC_FREQUENCY % 60
#if _check_periodic_freq_
#error Using HighSpeed Periodic: PERIODIC_FREQUENCY has to be a multiple of 60!
#endif
_60Hz++;
if (_60Hz >= (PERIODIC_FREQUENCY / 60))
{
_60Hz = 0;
}
else
{
return;
}
// Rest of the periodic function still runs at 60Hz like always
_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
#if CONTROL_RATE == 20
if (!_20Hz)
#endif
{
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
attitude_loop();
#endif
}
modules_periodic_task();
}
