C++ (Cpp) ahrs_update_fw_estimator 예제들

예제 #1

파일: main_ap.c 프로젝트: 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();
  }
}

예제 #2

파일: main_ap.c 프로젝트: 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

}