void autopilot_init(void)
{
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
autopilot_flight_time = 0;
autopilot_rc = true;
autopilot_power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
// init GNC stack
// TODO this should be done in modules init
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
#if USE_STABILIZATION_RATE
stabilization_rate_init();
#endif
stabilization_attitude_init();
// call implementation init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
// register messages
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
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 main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
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_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
autopilot_generated_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
// 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();
}
