C++ (Cpp) FLOAT_RMAT_COMP_INV Beispiele

Beispiel #1

Datei: ahrs_float_dcm.c Projekt: maxcht/paparazzi

/*
 * Compute body orientation and rates from imu orientation and rates
 */
static inline void compute_body_orientation_and_rates(void) {

  FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
                      ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
                      ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
  FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
  FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);

}

Beispiel #2

Datei: ahrs_float_dcm.c Projekt: F34140r/paparazzi

/*
 * Compute body orientation and rates from imu orientation and rates
 */
static inline void set_body_orientation_and_rates(void) {

  struct FloatRates body_rate;
  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
  stateSetBodyRates_f(&body_rate);

  struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
  FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
  FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);

  // Some stupid lines of code for neutrals
  struct FloatEulers ltp_to_body_euler;
  FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
  ltp_to_body_euler.phi -= ins_roll_neutral;
  ltp_to_body_euler.theta -= ins_pitch_neutral;
  stateSetNedToBodyEulers_f(&ltp_to_body_euler);

  // should be replaced at the end by:
  //   stateSetNedToBodyRMat_f(&ltp_to_body_rmat);

}

Beispiel #3

Datei: test_algebra.c Projekt: byrddev/paparazzi

float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) {

    struct FloatRMat ma2b_f;
    FLOAT_RMAT_COMP_INV(ma2b_f, ma2c_f, mb2c_f);
    struct Int32RMat ma2c_i;
    RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
    struct Int32RMat mb2c_i;
    RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
    struct Int32RMat ma2b_i;
    INT32_RMAT_COMP_INV(ma2b_i, ma2c_i, mb2c_i);

    struct FloatRMat err;
    RMAT_DIFF(err, ma2b_f, ma2b_i);
    float norm_err = FLOAT_RMAT_NORM(err);

    if (display) {
        printf("rmap comp_inv\n");
        DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
        DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
    }

    return norm_err;

}