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); }
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 autopilot_init(void) { /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; 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 autopilot_arming_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(); /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); 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 autopilot_init(void) { /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; 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 autopilot_arming_init(); nav_init(); guidance_h_init(); guidance_v_init(); stabilization_init(); stabilization_none_init(); stabilization_rate_init(); stabilization_attitude_init(); /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version); register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd); register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value); #ifdef ACTUATORS register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators); #endif #ifdef RADIO_CONTROL register_periodic_telemetry(DefaultPeriodic, "RC", send_rc); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc); #endif }
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 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; }