예제 #1
0
bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                      struct Int32Vect3 *lp_mag)
{

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

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

  ahrs_icq.status = AHRS_ICQ_RUNNING;
  ahrs_icq.is_aligned = TRUE;

  return TRUE;
}
예제 #2
0
static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
#else
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
  RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
  //  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
예제 #3
0
void imu_aspirin2_event(void)
{
  mpu60x0_spi_event(&imu_aspirin2.mpu);
  if (imu_aspirin2.mpu.data_available) {
    /* HMC5883 has xzy order of axes in returned data */
    struct Int32Vect3 mag;
    mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
    mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
    mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.p,
                 imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.x,
                 imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
    RATES_ASSIGN(imu.gyro_unscaled,
                 imu_aspirin2.mpu.data_rates.rates.p,
                 -imu_aspirin2.mpu.data_rates.rates.q,
                 -imu_aspirin2.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled,
                 imu_aspirin2.mpu.data_accel.vect.x,
                 -imu_aspirin2.mpu.data_accel.vect.y,
                 -imu_aspirin2.mpu.data_accel.vect.z);
    VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_COPY(imu.mag_unscaled, mag);
#endif
#else
    RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
    VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
    imu_aspirin2.mpu.data_available = FALSE;
    imu_aspirin2.gyro_valid = TRUE;
    imu_aspirin2.accel_valid = TRUE;
    imu_aspirin2.mag_valid = TRUE;
  }
}
예제 #4
0
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
{
  int32_t freq = (int32_t)(1. / dt);

  /* unbias gyro             */
  struct Int32Rates omega;
  RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias);

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

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

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

  // increase accel and mag propagation counters
  ahrs_icq.accel_cnt++;
  ahrs_icq.mag_cnt++;
}
예제 #5
0
void ahrs_align(void) {

#if USE_MAGNETOMETER
  /* 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);
  ahrs_impl.heading_aligned = TRUE;
#else
  /* Compute an initial orientation from accel and just set heading to zero */
  ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
  ahrs_impl.heading_aligned = FALSE;
#endif

  /* Convert initial orientation from quat to rotation matrix representations. */
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);

  /* Compute initial body orientation */
  compute_body_orientation_and_rates();

  /* 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;
}
예제 #6
0
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();

}
예제 #7
0
/**
 * Handle all the events of the Navstik IMU components.
 * When there is data available convert it to the correct axis and save it in the imu structure.
 */
void imu_navstik_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* MPU-60x0 event taks */
  mpu60x0_i2c_event(&imu_navstik.mpu);

  if (imu_navstik.mpu.data_available) {
    /* default orientation as should be printed on the pcb, z-down, ICs down */
    RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);

    imu_navstik.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_navstik.hmc);
  if (imu_navstik.hmc.data_available) {
    imu.mag_unscaled.x =  imu_navstik.hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_navstik.hmc.data.vect.z;
    imu_navstik.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}
예제 #8
0
void imu_aspirin_event(void)
{
  adxl345_spi_event(&imu_aspirin.acc_adxl);
  if (imu_aspirin.acc_adxl.data_available) {
    VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
    imu_aspirin.acc_adxl.data_available = FALSE;
    imu_aspirin.accel_valid = TRUE;
  }

  /* If the itg3200 I2C transaction has succeeded: convert the data */
  itg3200_event(&imu_aspirin.gyro_itg);
  if (imu_aspirin.gyro_itg.data_available) {
    RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
    imu_aspirin.gyro_itg.data_available = FALSE;
    imu_aspirin.gyro_valid = TRUE;
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_aspirin.mag_hmc);
  if (imu_aspirin.mag_hmc.data_available) {
#ifdef IMU_ASPIRIN_VERSION_1_0
    VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
    imu.mag_unscaled.x =  imu_aspirin.mag_hmc.data.vect.y;
    imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
    imu.mag_unscaled.z =  imu_aspirin.mag_hmc.data.vect.z;
#endif
    imu_aspirin.mag_hmc.data_available = FALSE;
    imu_aspirin.mag_valid = TRUE;
  }
}
예제 #9
0
파일: imu_navgo.c 프로젝트: AshuLara/lisa
void imu_navgo_event( void )
{

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event();
  if (itg3200_data_available) {
    RATES_COPY(imu.gyro_unscaled, itg3200_data);
    itg3200_data_available = FALSE;
    gyr_valid = TRUE;
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_event();
  if (adxl345_data_available) {
    VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
    adxl345_data_available = FALSE;
    acc_valid = TRUE;
  }

  // HMC58XX event task
  hmc58xx_event();
  if (hmc58xx_data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z);
    hmc58xx_data_available = FALSE;
    mag_valid = TRUE;
  }

}
예제 #10
0
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
  RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
  RATES_ADD(ahrs.imu_rate, omega);
  RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
#else
  RATES_COPY(ahrs.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.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
  INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);

  compute_imu_euler_and_rmat_from_quat();

  compute_body_orientation();

}
예제 #11
0
void imu_mpu_hmc_event(void)
{
    uint32_t now_ts = get_sys_time_usec();

    mpu60x0_spi_event(&imu_mpu_hmc.mpu);
    if (imu_mpu_hmc.mpu.data_available) {
        RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
        VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
        imu_mpu_hmc.mpu.data_available = false;
        imu_scale_gyro(&imu);
        imu_scale_accel(&imu);
        AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
        AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
    }

    /* HMC58XX event task */
    hmc58xx_event(&imu_mpu_hmc.hmc);
    if (imu_mpu_hmc.hmc.data_available) {
        /* mag rotated by 90deg around z axis relative to MPU */
        imu.mag_unscaled.x =  imu_mpu_hmc.hmc.data.vect.y;
        imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
        imu.mag_unscaled.z =  imu_mpu_hmc.hmc.data.vect.z;
        imu_mpu_hmc.hmc.data_available = false;
        imu_scale_mag(&imu);
        AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
    }
}
예제 #12
0
bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                     struct Int32Vect3 *lp_mag)
{

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

  /* Convert initial orientation from quat to rotation matrix representations. */
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);

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

  ahrs_fc.status = AHRS_FC_RUNNING;
  ahrs_fc.is_aligned = TRUE;

  return TRUE;
}
예제 #13
0
void ahrs_propagate(void) {

  /* converts gyro to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
  /* unbias measurement */
  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);

#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  const float alpha = 0.1;
  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
  RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif

  /* add correction     */
  struct FloatRates omega;
  RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
  /* and zeros it */
  FLOAT_RATES_ZERO(ahrs_impl.rate_correction);

  const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
  FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
  float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
  FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
  FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
  FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
  compute_body_orientation_and_rates();

}
예제 #14
0
void ahrs_align(void) {


  /* Compute an initial orientation using euler angles */
  ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  /* Convert initial orientation in quaternion and rotation matrice representations. */
  compute_imu_quat_and_rmat_from_euler();

  compute_body_orientation();

  /* 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;

}
예제 #15
0
/*
 * Compute body orientation and rates from imu orientation and rates
 */
void compute_body_orientation_and_rates(void) {

  /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */

  QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat);
  EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler);
  RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat);
  RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate);
}
예제 #16
0
파일: imu.c 프로젝트: KISSMonX/paparazzi
void WEAK imu_scale_gyro(struct Imu* _imu)
{
  RATES_COPY(_imu->gyro_prev, _imu->gyro);
  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
                  IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
                  IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
  _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
                  IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
}
예제 #17
0
void sim_overwrite_ahrs(void) {

  struct FloatQuat quat_f;
  QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
  stateSetNedToBodyQuat_f(&quat_f);

  struct FloatRates rates_f;
  RATES_COPY(rates_f, fdm.body_ecef_rotvel);
  stateSetBodyRates_f(&rates_f);

}
예제 #18
0
void imu_mpu_spi_event(void)
{
  mpu60x0_spi_event(&imu_mpu_spi.mpu);
  if (imu_mpu_spi.mpu.data_available) {
    RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
    imu_mpu_spi.mpu.data_available = FALSE;
    imu_mpu_spi.gyro_valid = TRUE;
    imu_mpu_spi.accel_valid = TRUE;
  }
}
예제 #19
0
static void feed_imu(int i) {
  if (i>0) {
    RATES_COPY(imu.gyro_prev, imu.gyro);
  }
  else {
    RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro);
  }
  RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
  ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
  MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
}
예제 #20
0
파일: imu.c 프로젝트: elemhsb/paparazzi
void WEAK imu_scale_gyro(struct Imu *_imu)
{
#ifdef TREF
	  RATES_COPY(_imu->gyro_prev, _imu->gyro);
	  _imu->gyro.p = ((_imu->gyro_unscaled.p + (TREF-_imu->temp_unscaled)*DXG -_imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
	                  IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
	  _imu->gyro.q = ((_imu->gyro_unscaled.q + (TREF-_imu->temp_unscaled)*DYG - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
	                  IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
	  _imu->gyro.r = ((_imu->gyro_unscaled.r + (TREF-_imu->temp_unscaled)*DZG - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
	                  IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
#else
  RATES_COPY(_imu->gyro_prev, _imu->gyro);
  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
                  IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
                  IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
  _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
                  IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
#endif // TREF
}
예제 #21
0
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;
}
void ahrs_propagate(void) {

  /* unbias gyro             */
  struct Int32Rates uf_rate;
  RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
#if USE_NOISE_CUT
  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
    /* low pass rate */
#if USE_NOISE_FILTER
    RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
    RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
#else
    RATES_ADD(ahrs.imu_rate, uf_rate);
    RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
#endif
#if USE_NOISE_CUT
  }
  RATES_COPY(last_uf_rate, uf_rate);
#endif

  /* integrate eulers */
  struct Int32Eulers euler_dot;
  INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
  EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);

  /* low pass measurement */
  EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
  EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);

  /* compute residual */
  EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler);
  INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);

  struct Int32Eulers correction;
  /* compute a correction */
  EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
  /* correct estimation */
  EULERS_ADD(ahrs_impl.hi_res_euler, correction);
  INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi);


  /* Compute LTP to IMU eulers      */
  EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);

  compute_imu_quat_and_rmat_from_euler();

  compute_body_orientation();

}
예제 #23
0
void imu_mpu_spi_event(void)
{
  mpu60x0_spi_event(&imu_mpu_spi.mpu);
  if (imu_mpu_spi.mpu.data_available) {
    uint32_t now_ts = get_sys_time_usec();
    RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
    imu_mpu_spi.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
  }
}
예제 #24
0
void ahrs_align(void)
{
  /* Compute an initial orientation from accel and mag directly as quaternion */
  ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);

  /* use average gyro as initial value for bias */
  struct FloatRates bias0;
  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
  RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);

  // ins and ahrs are now running
  ahrs.status = AHRS_RUNNING;
  ins.status = INS_RUNNING;
}
예제 #25
0
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;
}
예제 #26
0
void ahrs_ice_propagate(struct Int32Rates *gyro)
{

  /* unbias gyro             */
  struct Int32Rates uf_rate;
  RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
#if USE_NOISE_CUT
  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
#endif
    /* low pass rate */
#if USE_NOISE_FILTER
    RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN);
    RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1);
#else
    RATES_ADD(ahrs_ice.imu_rate, uf_rate);
    RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2);
#endif
#if USE_NOISE_CUT
  }
  RATES_COPY(last_uf_rate, uf_rate);
#endif

  /* integrate eulers */
  struct Int32Eulers euler_dot;
  int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate);
  EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);

  /* low pass measurement */
  EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement);
  EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2);

  /* compute residual */
  EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler);
  INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi);

  struct Int32Eulers correction;
  /* compute a correction */
  EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1);
  /* correct estimation */
  EULERS_ADD(ahrs_ice.hi_res_euler, correction);
  INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi);


  /* Compute LTP to IMU eulers      */
  EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
}
예제 #27
0
void imu_mpu9250_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU9250 I2C transaction has succeeded: convert the data
  mpu9250_i2c_event(&imu_mpu9250.mpu);

  if (imu_mpu9250.mpu.data_available) {
    // set channel order
    struct Int32Vect3 accel = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
    };
    struct Int32Rates rates = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
    };
    // unscaled vector
    VECT3_COPY(imu.accel_unscaled, accel);
    RATES_COPY(imu.gyro_unscaled, rates);

    imu_mpu9250.mpu.data_available = false;

    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
  }
#if IMU_MPU9250_READ_MAG
  // Test if mag data are updated
  if (imu_mpu9250.mpu.akm.data_available) {
    struct Int32Vect3 mag = {
      IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
      -IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
    };
    VECT3_COPY(imu.mag_unscaled, mag);
    imu_mpu9250.mpu.akm.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);

  }
#endif
}
예제 #28
0
void imu_mpu9250_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU9250 SPI transaction has succeeded: convert the data
  mpu9250_spi_event(&imu_mpu9250.mpu);

  if (imu_mpu9250.mpu.data_available) {
    // set channel order
    struct Int32Vect3 accel = {
      IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
    };
    struct Int32Rates rates = {
      IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
      IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
      IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
    };
    // unscaled vector
    VECT3_COPY(imu.accel_unscaled, accel);
    RATES_COPY(imu.gyro_unscaled, rates);

#if IMU_MPU9250_READ_MAG
    if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
      /** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
      struct Int32Vect3 mag;
      mag.x =  (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
      mag.y =  (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
      mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
      VECT3_COPY(imu.mag_unscaled, mag);
      imu_scale_mag(&imu);
      AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
    }
#endif

    imu_mpu9250.mpu.data_available = false;

    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
  }

}
예제 #29
0
void ahrs_align(void) {

  /* Compute an initial orientation using euler angles */
  ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
  /* Convert initial orientation in quaternion and rotation matrice representations. */
  FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
  FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
  /* Compute initial body orientation */
  compute_body_orientation_and_rates();

  /* 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;

}
예제 #30
0
void imu_drotek2_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  // If the MPU6050 I2C transaction has succeeded: convert the data
  mpu60x0_i2c_event(&imu_drotek2.mpu);

  if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
    /* change orientation, so if ICs face up, z-axis is down */
    imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
    imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
    imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
    imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
    imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
    imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
    /* default orientation as should be printed on the pcb, z-down, ICs down */
    RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
    VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif

    imu_drotek2.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
  }

  /* HMC58XX event task */
  hmc58xx_event(&imu_drotek2.hmc);
  if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
    imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
    imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
    imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
    VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
    imu_drotek2.hmc.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
  }
}