STATIC_INLINE void handle_periodic_tasks(void)
{
if (sys_time_check_and_ack_timer(main_periodic_tid)) {
main_periodic();
}
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
}
if (sys_time_check_and_ack_timer(radio_control_tid)) {
radio_control_periodic_task();
}
if (sys_time_check_and_ack_timer(failsafe_tid)) {
failsafe_check();
}
if (sys_time_check_and_ack_timer(electrical_tid)) {
electrical_periodic();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
telemetry_periodic();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
}
STATIC_INLINE void handle_periodic_tasks(void)
{
if (sys_time_check_and_ack_timer(main_periodic_tid)) {
main_periodic();
#if PERIODIC_FREQUENCY == MODULES_FREQUENCY
/* Use the main periodc freq timer for modules if the freqs are the same
* This is mainly useful for logging each step.
*/
modules_periodic_task();
#else
}
/* separate timer for modules, since it has a different freq than main */
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
#endif
}
if (sys_time_check_and_ack_timer(radio_control_tid)) {
radio_control_periodic_task();
}
if (sys_time_check_and_ack_timer(failsafe_tid)) {
failsafe_check();
}
if (sys_time_check_and_ack_timer(electrical_tid)) {
electrical_periodic();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
telemetry_periodic();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
}
STATIC_INLINE void handle_periodic_tasks( void ) {
if (sys_time_check_and_ack_timer(main_periodic_tid))
main_periodic();
if (sys_time_check_and_ack_timer(radio_control_tid))
radio_control_periodic_task();
if (sys_time_check_and_ack_timer(failsafe_tid))
failsafe_check();
if (sys_time_check_and_ack_timer(electrical_tid))
electrical_periodic();
if (sys_time_check_and_ack_timer(baro_tid))
baro_periodic();
if (sys_time_check_and_ack_timer(telemetry_tid))
telemetry_periodic();
}
STATIC_INLINE void main_init(void)
{
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
STATIC_INLINE void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
#ifndef INTER_MCU_AP
actuators_init();
#else
intermcu_init();
#endif
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
#ifndef INTER_MCU_AP
radio_control_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
/* temporary hack:
* Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
* This led to the problem that global waypoints were not "localized",
* so as a stop-gap measure we localize them all (again) here..
*/
waypoints_localize_all();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
#ifdef INTER_MCU_AP
intermcu_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
