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(); }
void init_ap(void) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ /** - start interrupt task */ mcu_int_enable(); #if defined(PPRZ_TRIG_INT_COMPR_FLASH) pprz_trig_int_init(); #endif /****** initialize and reset state interface ********/ stateInit(); /************* Sensors initialization ***************/ #if USE_AHRS ahrs_init(); #endif #if USE_BARO_BOARD baro_init(); #endif /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK link_mcu_init(); #endif /************ Internal status ***************/ autopilot_init(); modules_init(); // call autopilot implementation init after guidance modules init // it will set startup mode #if USE_GENERATED_AUTOPILOT autopilot_generated_init(); #else autopilot_static_init(); #endif settings_init(); /**** start timers for periodic functions *****/ sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL); #if !USE_GENERATED_AUTOPILOT navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL); #endif attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL); modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); #if USE_BARO_BOARD baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); #endif #if DOWNLINK downlink_init(); #endif /* set initial trim values. * these are passed to fbw via inter_mcu. */ PPRZ_MUTEX_LOCK(ap_state_mtx); ap_state->command_roll_trim = COMMAND_ROLL_TRIM; ap_state->command_pitch_trim = COMMAND_PITCH_TRIM; ap_state->command_yaw_trim = COMMAND_YAW_TRIM; PPRZ_MUTEX_UNLOCK(ap_state_mtx); #if USE_IMU // send body_to_imu from here for now AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); #endif }
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_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 #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); 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(); }