/********** INIT *************************************************************/
void init_fbw(void)
{
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
radio_control_init();
#endif /* RADIO_CONTROL */
#ifdef INTER_MCU
inter_mcu_init();
#endif /* INTER_MCU */
#if defined MCU_SPI_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif /* MCU_SPI_LINK || MCU_CAN_LINK */
#ifdef MCU_SPI_LINK
link_mcu_restart();
#endif /* MCU_SPI_LINK */
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
#endif /* RADIO_CONTROL */
#endif /* PERIODIC_TELEMETRY */
}
/********** INIT *************************************************************/
void init_fbw( void ) {
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_init();
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if DOWNLINK
register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status);
register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc);
#endif
#endif
}
/********** INIT *************************************************************/
void init_fbw( void ) //fbw初始化
{
mcu_init();//mcu初始化，如：led,uart,i2c,vocm,spi,dac
#if !(DISABLE_ELECTRICAL)
electrical_init();//供电初始化
#endif
#ifdef ACTUATORS
actuators_init();//执行器初始化
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();//无线电台初始化
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_init();
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
command_roll_trim = COMMAND_ROLL_TRIM;
command_pitch_trim = COMMAND_PITCH_TRIM;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
}
void init_ap( void )
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_BAROMETER
baro_init();
#endif
ins_init();
stateInit();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
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./60, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
sys_time_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
infrared_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_IMU
imu_init();
#endif
#ifdef USE_AHRS
ahrs_aligner_init();
ahrs_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
settings_init();
/** - start interrupt task */
mcu_int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_GPS
gps_init();
#endif
#if USE_IMU
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
#endif
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
ins_init();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
audio_telemetry_init();
#endif
/************ Internal status ***************/
autopilot_init();
h_ctl_init();
v_ctl_init();
nav_init();
modules_init();
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
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);
/** - start interrupt task */
mcu_int_enable();
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
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
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
hw_init();
sys_time_init();
#ifdef LED
led_init();
#endif
#ifdef ADC
adc_init();
#endif
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
ir_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef USE_UART2
Uart2Init();
#endif
#ifdef USE_UART3
Uart3Init();
#endif
#ifdef USE_USB_SERIAL
VCOM_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_I2C0
i2c0_init();
#endif
#ifdef USE_I2C1
i2c1_init();
#endif
#ifdef USE_I2C2
i2c2_init();
#endif
/************* Links initialization ***************/
#if defined USE_SPI
spi_init();
#endif
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
/** - start interrupt task */
int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
