Ejemplos de ahrs_update_accel en C++ (Cpp)

Ejemplo n.º 1

Archivo: run_ahrs_on_flight_log.c Proyecto: Abhi0204/paparazzi

int main(int argc, char** argv) {

  read_ascii_flight_log(IN_FILE);

  imu_init();
  ahrs_init();

  for (int i=0; i<nb_samples; i++) {
    feed_imu(i);
    if (ahrs.status == AHRS_UNINIT) {
      ahrs_aligner_run();
      if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
        ahrs_align();
    }
    else {
      ahrs_propagate((1./AHRS_PROPAGATE_FREQUENCY));
#ifdef ENABLE_MAG_UPDATE
      if (MAG_AVAILABLE(samples[i].flag))
        ahrs_update_mag();
#endif
#ifdef ENABLE_ACCEL_UPDATE
      if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag)))
        ahrs_update_accel();
#endif
    }
    store_filter_output(i);
  }

  dump_output(OUT_FILE);

}

Ejemplo n.º 2

Archivo: run_ahrs_on_synth_ivy.c Proyecto: AshuLara/paparazzi

gboolean timeout_callback(gpointer data) {

  for (int i=0; i<20; i++) {
    aos_compute_state();
    aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
    ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
    ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
    if (!(i%5)) ahrs_update_mag();
#endif
  }

#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
    EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif

#if AHRS_TYPE == AHRS_TYPE_ICQ
    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", 
	       ahrs_impl.gyro_bias.p,
	       ahrs_impl.gyro_bias.q,
	       ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
    struct Int32Rates bias_i;
    RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", 
	       bias_i.p,
	       bias_i.q,
	       bias_i.r);
#endif

  IvySendMsg("183 AHRS_EULER %f %f %f", 
	     ahrs_float.ltp_to_imu_euler.phi,
	     ahrs_float.ltp_to_imu_euler.theta,
	     ahrs_float.ltp_to_imu_euler.psi);

  IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
	     DegOfRad(aos.imu_rates.p), 
	     DegOfRad(aos.imu_rates.q), 
	     DegOfRad(aos.imu_rates.r),
	     DegOfRad(aos.ltp_to_imu_euler.phi), 
	     DegOfRad(aos.ltp_to_imu_euler.theta),
	     DegOfRad(aos.ltp_to_imu_euler.psi));

  IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
	     DegOfRad(aos.gyro_bias.p), 
	     DegOfRad(aos.gyro_bias.q), 
	     DegOfRad(aos.gyro_bias.r));

  return TRUE;
}

Ejemplo n.º 3

Archivo: main_ap.c Proyecto: agreee45/SNUT

static inline void on_gyro_accel_event( void ) {
  ImuScaleGyro(imu);
  ImuScaleAccel(imu);
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
      ahrs_align();
  }
  else {
    ahrs_propagate();
    ahrs_update_accel();
    ahrs_update_fw_estimator();
  }
}

Ejemplo n.º 4

Archivo: ahrs_on_synth.c Proyecto: EwoudSmeur/paparazzi

void aos_run(void)
{

  aos_compute_state();
  aos_compute_sensors();
#ifndef DISABLE_ALIGNEMENT
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
      ahrs_align();
    }
  } else {
#endif /* DISABLE_ALIGNEMENT */
    ahrs_propagate(aos.dt);
    ahrs_update_accel();

#ifndef DISABLE_MAG_UPDATE
    ahrs_update_mag();
#endif


#if AHRS_USE_GPS_HEADING

#if AHRS_TYPE == AHRS_TYPE_ICQ
    int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas);
#endif
#if AHRS_TYPE == AHRS_TYPE_FCQ
    float heading = aos.heading_meas;
#endif

#if AHRS_TYPE == AHRS_TYPE_FCR
    ahrs_impl.gps_course = aos.heading_meas;
    ahrs_impl.gps_course_valid = true;
#else
    if (aos.time > 10) {
      if (!ahrs_impl.heading_aligned) {
        ahrs_realign_heading(heading);
      } else {
        RunOnceEvery(100, ahrs_update_heading(heading));
      }
    }
#endif

#endif // AHRS_USE_GPS_HEADING

#ifndef DISABLE_ALIGNEMENT
  }
#endif

}

Ejemplo n.º 5

Archivo: ahrs_on_synth.c Proyecto: AshuLara/paparazzi

void aos_run(void) {

  aos_compute_state();
  aos_compute_sensors();
#ifndef DISABLE_ALIGNEMENT 
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
      ahrs_align();
  }
  else {
#endif /* DISABLE_ALIGNEMENT */
    ahrs_propagate();
    ahrs_update_accel();
    ahrs_update_mag();
#ifndef DISABLE_ALIGNEMENT 
  }
#endif

}

Ejemplo n.º 6

Archivo: main_ap.c Proyecto: lxl/paparazzi

static inline void on_gyro_accel_event( void ) {

#ifdef AHRS_CPU_LED
    LED_ON(AHRS_CPU_LED);
#endif

  // Run aligner on raw data as it also makes averages.
  if (ahrs.status == AHRS_UNINIT) {
    ImuScaleGyro(imu);
    ImuScaleAccel(imu);
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
      ahrs_align();
    return;
  }

#if PERIODIC_FREQUENCY == 60
  ImuScaleGyro(imu);
  ImuScaleAccel(imu);

  ahrs_propagate();
  ahrs_update_accel();
  ahrs_update_fw_estimator();

#else //PERIODIC_FREQUENCY
  static uint8_t _reduced_propagation_rate = 0;
  static uint8_t _reduced_correction_rate = 0;
  static struct Int32Vect3 acc_avg;
  static struct Int32Rates gyr_avg;

  RATES_ADD(gyr_avg, imu.gyro_unscaled);
  VECT3_ADD(acc_avg, imu.accel_unscaled);

  _reduced_propagation_rate++;
  if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
  {
  }
  else
  {
    _reduced_propagation_rate = 0;

    RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
    INT_RATES_ZERO(gyr_avg);

    ImuScaleGyro(imu);

    ahrs_propagate();

    _reduced_correction_rate++;
    if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
    {
      _reduced_correction_rate = 0;
      VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
      INT_VECT3_ZERO(acc_avg);
      ImuScaleAccel(imu);
      ahrs_update_accel();
      ahrs_update_fw_estimator();
    }
  }
#endif //PERIODIC_FREQUENCY

#ifdef AHRS_CPU_LED
    LED_OFF(AHRS_CPU_LED);
#endif

#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
  new_ins_attitude = 1;
#endif

}

Ejemplo n.º 7

Archivo: ahrs_float_lkf.c Proyecto: AntoineBlais/paparazzi

void ahrs_update(void) {
	ahrs_update_accel();
	ahrs_update_mag();
}