STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
air_data_init();
#if USE_BARO_BOARD
baro_init();
#endif
imu_init();
#if USE_IMU_FLOAT
imu_float_init();
#endif
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_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
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
// motor_mixing_init();
// servo_mixing_init();
set_servo_init();
#endif
radio_control_init();
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#if USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == UDP
udp_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);
baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
}
STATIC_INLINE void main_init( void ) {
#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
/* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
__asm("nop");
}
#endif
hw_init();
sys_time_init();
actuators_init();
radio_control_init();
booz2_analog_init();
baro_init();
#if defined USE_CAM || USE_DROP
booz2_pwm_init_hw();
#endif
battery_init();
imu_init();
// booz_fms_init(); // FIXME
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#ifdef USE_GPS
booz_gps_init();
#endif
modules_init();
int_enable();
}
STATIC_INLINE void main_init( void ) {
#ifndef NO_FUCKING_STARTUP_DELAY
#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
/* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
__asm("nop");
}
#endif
#endif
mcu_init();
sys_time_init();
electrical_init();
actuators_init();
radio_control_init();
#if DATALINK == XBEE
xbee_init();
#endif
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#ifdef USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
}
STATIC_INLINE void main_init( void ) {
mcu_init();
electrical_init();
actuators_init();
radio_control_init();
#if DATALINK == XBEE
xbee_init();
#endif
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
baro_init();
ins_init();
#if USE_GPS
gps_init();
#endif
modules_init();
settings_init();
mcu_int_enable();
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_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);
baro_tid = sys_time_register_timer(0.02, NULL);
telemetry_tid = sys_time_register_timer((1./60.), NULL);
}
static inline void autopilot_main_init( void ) {
hw_init();
sys_time_init();
led_init();
supervision_init();
actuators_init(ACTUATOR_BANK_MOTORS);
#if BOMB_ENABLED
bomb_init_servo(RADIO_FMS, 0, 0);
#endif
rc_init();
#if HARDWARE_ENABLED_GPS
gps_init();
#else
comm_init(COMM_0);
comm_add_tx_callback(COMM_0, comm_autopilot_message_send);
comm_add_rx_callback(COMM_0, comm_autopilot_message_received);
#endif
comm_init(COMM_TELEMETRY);
comm_add_tx_callback(COMM_TELEMETRY, comm_autopilot_message_send);
comm_add_rx_callback(COMM_TELEMETRY, comm_autopilot_message_received);
analog_init();
analog_enable_channel(ANALOG_CHANNEL_BATTERY);
analog_enable_channel(ANALOG_CHANNEL_PRESSURE);
altimeter_init();
imu_init();
autopilot_init();
ahrs_init();
ins_init();
fms_init();
int_enable();
}
static inline void main_init(void) {
hw_init();
sys_time_init();
imu_init();
baro_init();
radio_control_init();
actuators_init();
overo_link_init();
cscp_init();
adc_init();
#ifdef PASSTHROUGH_CYGNUS
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#endif
adc_buf_channel(0, &adc0_buf, 8);
adc_buf_channel(1, &adc1_buf, 8);
adc_buf_channel(2, &adc2_buf, 8);
adc_buf_channel(3, &adc3_buf, 8);
cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg);
new_radio_msg = FALSE;
new_baro_diff = FALSE;
new_baro_abs = FALSE;
new_vane = FALSE;
new_adc = FALSE;
overo_link.up.msg.imu_tick = 0;
}
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
}
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();
}
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
}
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();
}
