C++ (Cpp) ahrs_aligner_init示例

示例#1

0

显示文件

文件： demo_ahrs_actuators.c 项目： 2seasuav/paparuzzi

static inline void main_init(void)
{
  mcu_init();
  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);

  stateInit();
  actuators_init();

  imu_init();
#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif
  ahrs_init();

  settings_init();

  mcu_int_enable();

  downlink_init();

  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_COMMANDS, send_commands);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);

  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}

示例#2

0

显示文件

文件： main.c 项目： hulatang/paparazzi

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
}

示例#3

0

显示文件

文件： main.c 项目： jonasvanpelt/paparazzi

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);
}

示例#4

0

显示文件

文件： test_ahrs.c 项目： AshuLara/lisa

static inline void main_init( void ) {

  mcu_init();
  sys_time_init();
  imu_init();
  ahrs_aligner_init();
  ahrs_init();

  DEBUG_SERVO1_INIT();
  //  DEBUG_SERVO2_INIT();

  mcu_int_enable();
}

示例#5

0

显示文件

文件： test_ahrs.c 项目： AE4317group07/paparazzi

static inline void main_init(void)
{
  mcu_init();
  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
  imu_init();
#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif
  ahrs_init();
  downlink_init();

  mcu_int_enable();
}

示例#6

0

显示文件

文件： test_ahrs.c 项目： AxSt/paparazzi

static inline void main_init( void ) {

  mcu_init();
  sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
  imu_init();
  ahrs_aligner_init();
  ahrs_init();

  DEBUG_SERVO1_INIT();
  //  DEBUG_SERVO2_INIT();

  mcu_int_enable();
}

示例#7

0

显示文件

文件： main.c 项目： AntoineBlais/paparazzi

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();

}

示例#8

0

显示文件

文件： main.c 项目： dirkdokter/paparazzi

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();

}

示例#9

0

显示文件

文件： main.c 项目： BrandoJS/paparazzi

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);
}

示例#10

0

显示文件

文件： lisa_stm_passthrough_main.c 项目： AntoineBlais/paparazzi

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;
}

示例#11

0

显示文件

文件： main_ap.c 项目： WenFly123/openPPZ

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
}

示例#12

0

显示文件

文件： main.c 项目： budebulai/paparazzi

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();
}

示例#13

0

显示文件

文件： main_ap.c 项目： lxl/paparazzi

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
}

示例#14

0

显示文件

文件： main_ap.c 项目： hulatang/paparazzi

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;
}

示例#15

0

显示文件

文件： ahrs_on_synth.c 项目： Merafour/paparazzi

void aos_init(int traj_nb)
{

  aos.traj = &traj[traj_nb];

  aos.time = 0;
  aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY;
  aos.traj->ts = 0;
  aos.traj->ts = 1.; // default to one second

  /* default state */
  EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
  RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  FLOAT_VECT3_ZERO(aos.ltp_pos);
  FLOAT_VECT3_ZERO(aos.ltp_vel);
  FLOAT_VECT3_ZERO(aos.ltp_accel);
  FLOAT_VECT3_ZERO(aos.ltp_jerk);
  aos.traj->init_fun();

  imu_init();
  ahrs_init();
  ahrs_aligner_init();

#ifdef PERFECT_SENSORS
  RATES_ASSIGN(aos.gyro_bias,  RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
  aos.heading_noise = 0.;
#else
  RATES_ASSIGN(aos.gyro_bias,  RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
  RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
  VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
  aos.heading_noise = RadOfDeg(3.);
#endif


#ifdef FORCE_ALIGNEMENT
  //  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
  aos_compute_sensors();
  //  DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
  //  DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
  VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
  VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
  RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
  //  DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
  ahrs_align();
  //  DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);

#endif


#ifdef DISABLE_ALIGNEMENT
  printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
  printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
  printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
  printf("# DISABLE_MAG_UPDATE\n");
#endif
  printf("# AHRS_TYPE  %s\n", ahrs_type_str[AHRS_TYPE]);
  printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#if AHRS_MAG_UPDATE_YAW_ONLY
  printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
  printf("# PERFECT_SENSORS\n");
#endif
#if AHRS_USE_GPS_HEADING
  printf("# AHRS_USE_GPS_HEADING\n");
#endif
#if USE_AHRS_GPS_ACCELERATIONS
  printf("# USE_AHRS_GPS_ACCELERATIONS\n");
#endif

  printf("# tajectory : %s\n", aos.traj->name);

};

示例#16

0

显示文件

文件： main.c 项目： paparazzi/paparazzi

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();
}