C++ (Cpp) set_body_state_from_quat Beispiele

Beispiel #1

Datei: ahrs_int_cmpl_quat.c Projekt: AULA-PPZ/paparazzi

void ahrs_propagate(float dt) {
  int32_t freq = (int32_t)(1./dt);

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);

  /* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate,2);
  RATES_ADD(ahrs_impl.imu_rate, omega);
  RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 3);
#else
  RATES_COPY(ahrs_impl.imu_rate, omega);
#endif

  /* add correction */
  RATES_ADD(omega, ahrs_impl.rate_correction);
  /* and zeros it */
  INT_RATES_ZERO(ahrs_impl.rate_correction);

  /* integrate quaternion */
  int32_quat_integrate_fi(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.high_rez_quat,
                          &omega, freq);
  int32_quat_normalize(&ahrs_impl.ltp_to_imu_quat);

  set_body_state_from_quat();

  // increase accel and mag propagation counters
  ahrs_impl.accel_cnt++;
  ahrs_impl.mag_cnt++;
}

Beispiel #2

Datei: ahrs_int_cmpl_quat.c Projekt: jeffchenhr/dev-barometer

void ahrs_propagate(void) {

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);

  /* low pass rate */
//#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  if (gyro_lowpass_filter > 1) {
	  RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1);
	  RATES_ADD(ahrs_impl.imu_rate, omega);
	  RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter);
  //#else
  } else {
	  RATES_COPY(ahrs_impl.imu_rate, omega);
  //#endif
  }

  /* add correction     */
  RATES_ADD(omega, ahrs_impl.rate_correction);
  /* and zeros it */
  INT_RATES_ZERO(ahrs_impl.rate_correction);

  /* integrate quaternion */
  INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
  INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);

  set_body_state_from_quat();

}

Beispiel #3

Datei: ahrs_float_mlkf.c Projekt: Aishwaryabr/paparazzi

void ahrs_align(void) {

  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);

  /* set initial body orientation */
  set_body_state_from_quat();

  /* used averaged gyro as initial value for bias */
  struct Int32Rates bias0;
  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
  RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);

  ahrs.status = AHRS_RUNNING;
}

Beispiel #4

Datei: ahrs_int_cmpl_quat.c Projekt: jeffchenhr/dev-barometer

void ahrs_align(void) {

#if USE_MAGNETOMETER
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  ahrs_impl.heading_aligned = TRUE;
#else
  /* Compute an initial orientation from accel and just set heading to zero */
  ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
  ahrs_impl.heading_aligned = FALSE;
#endif

  set_body_state_from_quat();

  /* Use low passed gyro value as initial bias */
  RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
  RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
  INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);

  ahrs.status = AHRS_RUNNING;
}

Beispiel #5

Datei: ahrs_float_mlkf.c Projekt: Aishwaryabr/paparazzi

void ahrs_propagate(void) {
  propagate_ref();
  propagate_state();
  set_body_state_from_quat();
}