void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
{
  // realign all the filter if needed
  // a complete init cycle is required
  if (ahrs_float_inv.reset) {
    ahrs_float_inv.reset = false;
    ahrs_float_inv.is_aligned = false;
    init_invariant_state();
  }

  // fill command vector
  struct FloatRates gyro_meas_body;
  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
  float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
  ahrs_float_inv.cmd.rates = gyro_meas_body;

  // update correction gains
  error_output(&ahrs_float_inv);

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

  // normalize quaternion
  float_quat_normalize(&ahrs_float_inv.state.quat);

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

#if SEND_INVARIANT_FILTER
  struct FloatEulers eulers;
  float foo = 0.f;
  float_eulers_of_quat(&eulers, &ahrs_float_inv.state.quat);
  RunOnceEvery(3,
      pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
        AC_ID,
        &ahrs_float_inv.state.quat.qi,
        &eulers.phi,
        &eulers.theta,
        &eulers.psi,
        &foo,
        &foo,
        &foo,
        &foo,
        &foo,
        &foo,
        &ahrs_float_inv.state.bias.p,
        &ahrs_float_inv.state.bias.q,
        &ahrs_float_inv.state.bias.r,
        &ahrs_float_inv.state.as,
        &ahrs_float_inv.state.cs,
        &foo,
        &foo);
      );
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
}
示例#3
0
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
}