C++ (Cpp) float_quat_invert Examples

Example #1

File: energy_ctrl.c Project: amtcvx/paparazzi

static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
                           struct FloatQuat *q_b2i_f)
{
  float_quat_invert(&imu_to_body_quat, q_b2i_f);
}

Example #2

File: ins_float_invariant.c Project: hannwoei/paparazzi

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
}