static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_body_quat);
  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.body_rate);
#else
  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_imu_quat);
  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
  RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
  //  memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate);
#else
  struct FloatEulers eul_f;
  EULERS_FLOAT_OF_BFP(eul_f, ahrs_impl.ltp_to_imu_euler);
  FLOAT_QUAT_OF_EULERS(output[i].quat_est, eul_f);
  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
  RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
  //  memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
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_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
  const float dt = 1./512.;
  /* add correction     */
  struct FloatRates omega;
  RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
  //  DISPLAY_FLOAT_RATES("omega ", omega);
  /* and zeros it */
  FLOAT_RATES_ZERO(ahrs_impl.rate_correction);

  /* first order integration of rotation matrix */
  struct FloatRMat exp_omega_dt = {
    { 1.        ,  dt*omega.r, -dt*omega.q,
     -dt*omega.r,  1.        ,  dt*omega.p,
      dt*omega.q, -dt*omega.p,  1.                       }};
  struct FloatRMat R_tdt;
  FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
  memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));

  float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
  //  struct FloatRMat test;
  //  FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
  //  DISPLAY_FLOAT_RMAT("foo", test);

  compute_imu_quat_and_euler_from_rmat();
  compute_body_orientation_and_rates();

}
Exemple #4
0
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
                         &gyro_float.p, &gyro_float.q, &gyro_float.r);
}
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;
}
Exemple #6
0
void ahrs_propagate(void)
{
  /* convert imu data to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);

  /* unbias rate measurement */
  RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);

  /* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
  float dp=0,dq=0,dr=0;
  dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q;
  dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R;
  dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P;
  dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R;
  dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P;
  dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q;

  ahrs_float.imu_rate.p += dp;
  ahrs_float.imu_rate.q += dq;
  ahrs_float.imu_rate.r += dr;
#endif

  Matrix_update();
  // INFO, ahrs struct only updated in ahrs_update_fw_estimator

  Normalize();
}
static void dialog_with_io_proc() {

  struct AutopilotMessageCRCFrame msg_in;
  struct AutopilotMessageCRCFrame msg_out;
  uint8_t crc_valid;

  for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
//  for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i];

  spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);

  struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up;
  RATES_FLOAT_OF_BFP(otp.imu.gyro, in->gyro);
  ACCELS_FLOAT_OF_BFP(otp.imu.accel, in->accel);
  MAGS_FLOAT_OF_BFP(otp.imu.mag, in->mag);

  otp.io_proc_msg_cnt = in->stm_msg_cnt;
  otp.io_proc_err_cnt = in->stm_crc_err_cnt;

  otp.rc_status = in->rc_status;

  otp.baro_abs  = in->pressure_absolute;
  otp.baro_diff = in->pressure_differential;

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

}
Exemple #10
0
void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
{
  /* convert imu data to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, *gyro);

  /* unbias rate measurement */
  RATES_DIFF(ahrs_dcm.imu_rate, gyro_float, ahrs_dcm.gyro_bias);

  /* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
  float dp = 0, dq = 0, dr = 0;
  dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
  dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
  dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
  dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
  dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
  dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;

  ahrs_dcm.imu_rate.p += dp;
  ahrs_dcm.imu_rate.q += dq;
  ahrs_dcm.imu_rate.r += dr;
#endif

  Matrix_update(dt);

  Normalize();

  compute_ahrs_representations();
}
Exemple #11
0
void stateCalcBodyRates_f(void) {
  if (bit_is_set(state.rate_status, RATE_F))
    return;

  if (bit_is_set(state.rate_status, RATE_I)) {
    RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.rate_status, RATE_F);
}
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;
}
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;
}
void print_raw_log_entry(struct raw_log_entry* e){
  struct DoubleVect3 tempvect;
  struct DoubleRates temprates;
  printf("%f\t", e->time);
	printf("%i\t", e->message.valid_sensors);
  RATES_FLOAT_OF_BFP(temprates, e->message.gyro);
	printf("% f % f % f\t", temprates.p,     temprates.q,     temprates.r);
  ACCELS_FLOAT_OF_BFP(tempvect, e->message.accel);
	printf("% f % f % f\t", tempvect.x,    tempvect.y,    tempvect.z);
  MAGS_FLOAT_OF_BFP(tempvect, e->message.mag);
	printf("% f % f % f\t", tempvect.x,      tempvect.y,     tempvect.z);
	printf("% i % i % i\t", e->message.ecef_pos.x, e->message.ecef_pos.y, e->message.ecef_pos.z);
	printf("% i % i % i\t", e->message.ecef_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z);
  double baro_scaled = (double)(e->message.pressure_absolute)/256;
	printf("%f", baro_scaled);
}
void ahrs_update_fw_estimator(void)
{
  struct FloatEulers att;
  // export results to estimator
  EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler);

  estimator_phi   = att.phi - ins_roll_neutral;
  estimator_theta = att.theta - ins_pitch_neutral;
  estimator_psi   = att.psi;

  struct FloatRates rates;
  RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
  estimator_p = rates.p;
  estimator_q = rates.q;
  estimator_r = rates.r;

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

}
Exemple #17
0
void ahrs_propagate(void) {
  /* compute unbiased rates */
  RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
  RATES_SUB(bafe_rates, bafe_bias);

  /* compute F
     F is only needed later on to update the state covariance P.
     However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
     compute the time derivative of the quaternion, so we compute F now
  */

  /* Fill in Wxq(pqr) into F */
  bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
  bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
  bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
  bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;

  bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
  bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
  bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
  /* Fill in [4:6][0:3] region */
  bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
  bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
  bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;

  bafe_F[1][4] =  bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
  bafe_F[3][5] = -bafe_F[0][4];
  bafe_F[1][6] = -bafe_F[0][5];
  bafe_F[2][4] = -bafe_F[0][6];
  /* quat_dot = Wxq(pqr) * quat */
  bafe_qdot.qi=                           bafe_F[0][1]*bafe_quat.qx+bafe_F[0][2]*bafe_quat.qy+bafe_F[0][3] * bafe_quat.qz;
  bafe_qdot.qx= bafe_F[1][0]*bafe_quat.qi                          +bafe_F[1][2]*bafe_quat.qy+bafe_F[1][3] * bafe_quat.qz;
  bafe_qdot.qy= bafe_F[2][0]*bafe_quat.qi+bafe_F[2][1]*bafe_quat.qx                          +bafe_F[2][3] * bafe_quat.qz;
  bafe_qdot.qz= bafe_F[3][0]*bafe_quat.qi+bafe_F[3][1]*bafe_quat.qx+bafe_F[3][2]*bafe_quat.qy                            ;
  /* propagate quaternion */
  bafe_quat.qi += bafe_qdot.qi * BAFE_DT;
  bafe_quat.qx += bafe_qdot.qx * BAFE_DT;
  bafe_quat.qy += bafe_qdot.qy * BAFE_DT;
  bafe_quat.qz += bafe_qdot.qz * BAFE_DT;


}
Exemple #18
0
static inline void propagate_ref(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
  /* lowpass angular 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


  /* propagate reference quaternion only if rate is non null */
  const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
  if (no > FLT_MIN) {
    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
    const float a  = 0.5*no*dt;
    const float ca = cosf(a);
    const float sa_ov_no = sinf(a)/no;
    const float dp = sa_ov_no*ahrs_impl.imu_rate.p;
    const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
    const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
    const float qi = ahrs_impl.ltp_to_imu_quat.qi;
    const float qx = ahrs_impl.ltp_to_imu_quat.qx;
    const float qy = ahrs_impl.ltp_to_imu_quat.qy;
    const float qz = ahrs_impl.ltp_to_imu_quat.qz;
    ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
    ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
    ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
    ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;

    //    printf("%f\n",  ahrs_impl.ltp_to_imu_quat.qi);
  }

}
static inline void propagate_ref(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
  /* lowpass angular 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

  /* propagate reference quaternion */
  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
  FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt);

}
void ins_reset_altitude_ref(void)
{
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm = state.utm_origin_f;
  utm.alt = gps.hmsl / 1000.0f;
  stateSetLocalUtmOrigin_f(&utm);
#else
  struct LlaCoor_i lla = {
    .lat = state.ned_origin_i.lla.lat,
    .lon = state.ned_origin_i.lla.lon,
    .alt = gps.lla_pos.alt
  };
  struct LtpDef_i ltp_def;
  ltp_def_from_lla_i(&ltp_def, &lla);
  ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ltp_def);
#endif
}

void ahrs_init(void)
{
  ahrs.status = AHRS_UNINIT;
}

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;
}
Exemple #21
0
bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                      struct Int32Vect3 *lp_mag)
{
  /* Compute an initial orientation using euler angles */
  ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);

  /* Convert initial orientation in quaternion and rotation matrice representations. */
  struct FloatRMat ltp_to_imu_rmat;
  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);

  /* set filter dcm */
  set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);

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

  ahrs_dcm.status = AHRS_DCM_RUNNING;
  ahrs_dcm.is_aligned = TRUE;

  return TRUE;
}
Exemple #22
0
/**
 * copy imu.gyro into gyro_float
 * compute imu_rate without bias
 * scale values to [rad/s]
 * Update Matrix
 * Normalize
 */
void ahrs_propagate(void)
{
  struct FloatRates gyro_float;

  /* convert bfp imu data to floating point */
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); // olri div by (1<<12)

  /* unbias rate measurement */
  RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);

  // scale gyro adc raw to [rad/s] FIXME // olri
  ahrs_float.imu_rate.p /= 61.35f;
  ahrs_float.imu_rate.q /= 57.96f;
  ahrs_float.imu_rate.r /= 60.10f;

  /* Update Matrix */
  Matrix_update();

  /* Normalize */
  Normalize();

  // INFO, ahrs struct only updated in ahrs_update_fw_estimator
}
Exemple #23
0
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{

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

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

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

#if AHRS_PROPAGATE_RMAT
  float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
  float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
  float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
  float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
  float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif

  // increase accel and mag propagation counters
  ahrs_fc.accel_cnt++;
  ahrs_fc.mag_cnt++;
}
/*
 * Propagate our dynamic system in time
 *
 * Run the strapdown computation and the predict step of the filter
 *
 *
 *      quat_dot = Wxq(pqr) * quat
 *      bias_dot = 0
 *
 * Wxq is the quaternion omega matrix:
 *
 *              [ 0, -p, -q, -r ]
 *      1/2 *   [ p,  0,  r, -q ]
 *              [ q, -r,  0,  p ]
 *              [ r,  q, -p,  0 ]
 *
 */
void ahrs_propagate(void) {
	int i, j, k;

    ahrs_lowpass_accel();

	/* compute unbiased rates */
	RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
	RATES_SUB(bafl_rates, bafl_bias);


	/* run strapdown computation
	 *
	 */

	/* multiplicative quaternion update */
	/* compute correction quaternion */
	QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
	/* normalize it. maybe not necessary?? */
	float q_sq;
	q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
	if (q_sq > 1) { /* this should actually never happen */
		FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
	} else {
		bafl_qr.qi = sqrtf(1 - q_sq);
	}
	/* propagate correction quaternion */
	FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);

	bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
	//TODO check if broot force normalization is good, use lagrange normalization?
	FLOAT_QUAT_NORMALISE(bafl_quat);


	/*
	 *  compute all representations
	 */
	/* maintain rotation matrix representation */
	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
	/* maintain euler representation */
	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
	AHRS_TO_BFP();
	AHRS_LTP_TO_BODY();


	/* error KF-Filter
	 * propagate only the error covariance matrix since error is corrected after each measurement
	 *
	 * F = [ 0  0  0          ]
	 *     [ 0  0  0   -Cbn   ]
	 *     [ 0  0  0          ]
	 *     [ 0  0  0  0  0  0 ]
	 *     [ 0  0  0  0  0  0 ]
	 *     [ 0  0  0  0  0  0 ]
	 *
	 * T = e^(dt * F)
	 *
	 * T = [ 1   0   0             ]
	 *     [ 0   1   0   -Cbn*dt   ]
	 *     [ 0   0   1             ]
	 *     [ 0   0   0   1   0   0 ]
	 *     [ 0   0   0   0   1   0 ]
	 *     [ 0   0   0   0   0   1 ]
	 *
	 * P_prio = T * P * T_T + Q
	 *
	 */

	/*
	 *  compute state transition matrix T
	 *
	 *  diagonal elements of T are always one
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
		}
	}


	/*
	 * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
	 */
	/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6)      */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_tempP[i][j] = 0.;
			for (k = 0; k < BAFL_SSIZE; k++) {
				bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
			}
		}
	}

	/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q  */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_P[i][j] = 0.;
			for (k = 0; k < BAFL_SSIZE; k++) {
				bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
			}
		}
		if (i<3) {
			bafl_P[i][i] += bafl_Q_att;
		} else {
			bafl_P[i][i] += bafl_Q_gyro;
		}
	}

#ifdef LKF_PRINT_P
	printf("Pp =");
	for (i = 0; i < 6; i++) {
		printf("[");
		for (j = 0; j < 6; j++) {
			printf("%f\t", bafl_P[i][j]);
		}
		printf("]\n    ");
	}
	printf("\n");
#endif
}
void ahrs_propagate(void) {
  struct NedCoor_f accel;
  struct FloatRates body_rates;
  struct FloatEulers eulers;

  // realign all the filter if needed
  // a complete init cycle is required
  if (ins_impl.reset) {
    ins_impl.reset = FALSE;
    ins.status = INS_UNINIT;
    ahrs.status = AHRS_UNINIT;
    init_invariant_state();
  }

  // fill command vector
  struct Int32Rates gyro_meas_body;
  INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
  struct Int32Vect3 accel_meas_body;
  INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);

  // update correction gains
  error_output(&ins_impl);

  // propagate model
  struct inv_state new_state;
  runge_kutta_4_float((float*)&new_state,
      (float*)&ins_impl.state, INV_STATE_DIM,
      (float*)&ins_impl.cmd, INV_COMMAND_DIM,
      invariant_model, dt);
  ins_impl.state = new_state;

  // normalize quaternion
  FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);

  // set global state
  FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
  // Some stupid lines of code for neutrals
  eulers.phi -= ins_roll_neutral;
  eulers.theta -= ins_pitch_neutral;
  stateSetNedToBodyEulers_f(&eulers);
#else
  stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
  RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
  stateSetBodyRates_f(&body_rates);
  stateSetPositionNed_f(&ins_impl.state.pos);
  stateSetSpeedNed_f(&ins_impl.state.speed);
  // untilt accel and remove gravity
  FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
  FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
  FLOAT_VECT3_ADD(accel, A);
  stateSetAccelNed_f(&accel);

  //------------------------------------------------------------//

  RunOnceEvery(3,{
      DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
        &ins_impl.state.quat.qi,
        &eulers.phi,
        &eulers.theta,
        &eulers.psi,
        &ins_impl.state.speed.x,
        &ins_impl.state.speed.y,
        &ins_impl.state.speed.z,
        &ins_impl.state.pos.x,
        &ins_impl.state.pos.y,
        &ins_impl.state.pos.z,
        &ins_impl.state.bias.p,
        &ins_impl.state.bias.q,
        &ins_impl.state.bias.r,
        &ins_impl.state.as,
        &ins_impl.state.hb,
        &ins_impl.meas.baro_alt,
        &ins_impl.meas.pos_gps.z)
      });

#if LOG_INVARIANT_FILTER
  if (pprzLogFile.fs != NULL) {
    if (!log_started) {
      // log file header
      sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
      log_started = TRUE;
    }
    else {
      sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
          ins_impl.cmd.rates.p,
          ins_impl.cmd.rates.q,
          ins_impl.cmd.rates.r,
          ins_impl.cmd.accel.x,
          ins_impl.cmd.accel.y,
          ins_impl.cmd.accel.z,
          ins_impl.meas.pos_gps.x,
          ins_impl.meas.pos_gps.y,
          ins_impl.meas.pos_gps.z,
          ins_impl.meas.speed_gps.x,
          ins_impl.meas.speed_gps.y,
          ins_impl.meas.speed_gps.z,
          ins_impl.meas.mag.x,
          ins_impl.meas.mag.y,
          ins_impl.meas.mag.z,
          ins_impl.meas.baro_alt,
          ins_impl.state.quat.qi,
          ins_impl.state.quat.qx,
          ins_impl.state.quat.qy,
          ins_impl.state.quat.qz,
          ins_impl.state.bias.p,
          ins_impl.state.bias.q,
          ins_impl.state.bias.r,
          ins_impl.state.speed.x,
          ins_impl.state.speed.y,
          ins_impl.state.speed.z,
          ins_impl.state.pos.x,
          ins_impl.state.pos.y,
          ins_impl.state.pos.z,
          ins_impl.state.hb,
          ins_impl.state.as);
    }
  }
#endif
}
static void report(void) {

  int output_sensors = FALSE;
  int output_pos = FALSE;

  printf("%f ", aos.time);

  printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi),
                DegOfRad(aos.ltp_to_imu_euler.theta),
                DegOfRad(aos.ltp_to_imu_euler.psi));

  printf("%f %f %f ", DegOfRad(aos.imu_rates.p),
                DegOfRad(aos.imu_rates.q),
                DegOfRad(aos.imu_rates.r));

  printf("%f %f %f ", DegOfRad(aos.gyro_bias.p),
                DegOfRad(aos.gyro_bias.q),
                DegOfRad(aos.gyro_bias.r));

#if AHRS_TYPE == AHRS_TYPE_ICQ
  struct Int32Eulers ltp_to_imu_euler_i;
  int32_eulers_of_quat(&ltp_to_imu_euler_i, &ahrs_impl.ltp_to_imu_quat);
  struct FloatEulers ltp_to_imu_euler_f;
  EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i);
  printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi),
         DegOfRad(ltp_to_imu_euler_f.theta),
         DegOfRad(ltp_to_imu_euler_f.psi));

  struct FloatRates imu_rate_f;
  RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate);
  printf("%f %f %f ", DegOfRad(imu_rate_f.p),
         DegOfRad(imu_rate_f.q),
         DegOfRad(imu_rate_f.r));

  struct FloatRates bias;
  RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
  printf("%f %f %f ", DegOfRad(bias.p),
               DegOfRad(bias.q),
               DegOfRad(bias.r));

#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR

  printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi),
         DegOfRad(ahrs_impl.ltp_to_imu_euler.theta),
         DegOfRad(ahrs_impl.ltp_to_imu_euler.psi));

  printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p),
         DegOfRad(ahrs_impl.imu_rate.q),
         DegOfRad(ahrs_impl.imu_rate.r));

  printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
               DegOfRad(ahrs_impl.gyro_bias.q),
               DegOfRad(ahrs_impl.gyro_bias.r));
#endif

  if (output_pos) {
    printf("%f %f %f ", aos.ltp_pos.x,
                        aos.ltp_pos.y,
                  aos.ltp_pos.z);
  }

  if (output_sensors) {

  }

  printf("\n");

}
void ahrs_propagate(float dt)
{
  struct FloatVect3 accel;
  struct FloatRates body_rates;

  // realign all the filter if needed
  // a complete init cycle is required
  if (ins_impl.reset) {
    ins_impl.reset = FALSE;
    ins.status = INS_UNINIT;
    ahrs.status = AHRS_UNINIT;
    init_invariant_state();
  }

  // fill command vector
  struct Int32Rates gyro_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro);
  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
  struct Int32Vect3 accel_meas_body;
  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);

  // update correction gains
  error_output(&ins_impl);

  // propagate model
  struct inv_state new_state;
  runge_kutta_4_float((float *)&new_state,
                      (float *)&ins_impl.state, INV_STATE_DIM,
                      (float *)&ins_impl.cmd, INV_COMMAND_DIM,
                      invariant_model, dt);
  ins_impl.state = new_state;

  // normalize quaternion
  FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);

  // set global state
  stateSetNedToBodyQuat_f(&ins_impl.state.quat);
  RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
  stateSetBodyRates_f(&body_rates);
  stateSetPositionNed_f(&ins_impl.state.pos);
  stateSetSpeedNed_f(&ins_impl.state.speed);
  // untilt accel and remove gravity
  struct FloatQuat q_b2n;
  float_quat_invert(&q_b2n, &ins_impl.state.quat);
  float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel);
  VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
  VECT3_ADD(accel, A);
  stateSetAccelNed_f((struct NedCoor_f *)&accel);

  //------------------------------------------------------------//

#if SEND_INVARIANT_FILTER
  struct FloatEulers eulers;
  FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
  RunOnceEvery(3, {
    pprz_msg_send_INV_FILTER(trans, dev, AC_ID,
    &ins_impl.state.quat.qi,
    &eulers.phi,
    &eulers.theta,
    &eulers.psi,
    &ins_impl.state.speed.x,
    &ins_impl.state.speed.y,
    &ins_impl.state.speed.z,
    &ins_impl.state.pos.x,
    &ins_impl.state.pos.y,
    &ins_impl.state.pos.z,
    &ins_impl.state.bias.p,
    &ins_impl.state.bias.q,
    &ins_impl.state.bias.r,
    &ins_impl.state.as,
    &ins_impl.state.hb,
    &ins_impl.meas.baro_alt,
    &ins_impl.meas.pos_gps.z)
  });
#endif

#if LOG_INVARIANT_FILTER
  if (pprzLogFile.fs != NULL) {
    if (!log_started) {
      // log file header
      sdLogWriteLog(&pprzLogFile,
                    "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
      log_started = TRUE;
    } else {
      sdLogWriteLog(&pprzLogFile,
                    "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
                    ins_impl.cmd.rates.p,
                    ins_impl.cmd.rates.q,
                    ins_impl.cmd.rates.r,
                    ins_impl.cmd.accel.x,
                    ins_impl.cmd.accel.y,
                    ins_impl.cmd.accel.z,
                    ins_impl.meas.pos_gps.x,
                    ins_impl.meas.pos_gps.y,
                    ins_impl.meas.pos_gps.z,
                    ins_impl.meas.speed_gps.x,
                    ins_impl.meas.speed_gps.y,
                    ins_impl.meas.speed_gps.z,
                    ins_impl.meas.mag.x,
                    ins_impl.meas.mag.y,
                    ins_impl.meas.mag.z,
                    ins_impl.meas.baro_alt,
                    ins_impl.state.quat.qi,
                    ins_impl.state.quat.qx,
                    ins_impl.state.quat.qy,
                    ins_impl.state.quat.qz,
                    ins_impl.state.bias.p,
                    ins_impl.state.bias.q,
                    ins_impl.state.bias.r,
                    ins_impl.state.speed.x,
                    ins_impl.state.speed.y,
                    ins_impl.state.speed.z,
                    ins_impl.state.pos.x,
                    ins_impl.state.pos.y,
                    ins_impl.state.pos.z,
                    ins_impl.state.hb,
                    ins_impl.state.as);
    }
  }
#endif
}
void ahrs_align(void) {
	RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
	ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
	ahrs.status = AHRS_RUNNING;
}
Exemple #29
0
static void send_gyro(void) {
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
  DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
      &gyro_float.p, &gyro_float.q, &gyro_float.r);
}