static inline void copy_from_to_fbw(void)
{
PPRZ_MUTEX_LOCK(fbw_state_mtx);
PPRZ_MUTEX_LOCK(ap_state_mtx);
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
#elif defined RADIO_YAW && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
}
/**
* Handles inter_mcu event
*/
void inter_mcu_event_handle(void)
{
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
pre_inter_mcu_received_ap();
if (inter_mcu_received_ap) {
inter_mcu_received_ap = false;
inter_mcu_event_task();
PPRZ_MUTEX_LOCK(ap_state_mtx);
command_roll_trim = ap_state->command_roll_trim;
command_pitch_trim = ap_state->command_pitch_trim;
command_yaw_trim = ap_state->command_yaw_trim;
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE;
}
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
} else {
#if SET_AP_ONLY_COMMANDS
SetApOnlyCommands(ap_state->commands);
#endif /* SET_AP_ONLY_COMMANDS */
}
fbw_new_actuators = 1;
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */
}
post_inter_mcu_received_ap();
update_actuators();
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = false;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
}
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
}
