Example #1
0
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);
}
Example #2
0
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
}
Example #3
0
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
}
Example #4
0
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
}
Example #5
0
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();

}
Example #6
0
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();

}
Example #7
0
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;
}