void orientationCalcRMat_i(struct OrientationReps* orientation) {
  if (bit_is_set(orientation->status, ORREP_RMAT_I))
    return;

  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
    SetBit(orientation->status, ORREP_QUAT_I);
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
    SetBit(orientation->status, ORREP_EULER_I);
    INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_RMAT_I);
}
Esempio n. 2
0
/* Compute ltp to imu rotation in euler angles and rotation matrice representation
   from the quaternion representation */
__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {

  /* Compute LTP to IMU euler */
  INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat);
  /* Compute LTP to IMU rotation matrix */
  INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat);

}
void sim_overwrite_ahrs(void) {

  EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);

  QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);

  RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);

  INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);

}
Esempio n. 4
0
static void test_2(void) {

    struct Int32Vect3 v1 = { 5000, 5000, 5000 };
    DISPLAY_INT32_VECT3("v1", v1);

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);
    INT32_QUAT_NORMALIZE(quat_i);
    DISPLAY_INT32_QUAT("quat_i_n", quat_i);

    struct Int32Vect3 v2;
    INT32_QUAT_VMULT(v2, quat_i, v1);
    DISPLAY_INT32_VECT3("v2", v2);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

    struct Int32Vect3 v3;
    INT32_RMAT_VMULT(v3, rmat_i, v1);
    DISPLAY_INT32_VECT3("v3", v3);

    struct Int32RMat rmat_i2;
    INT32_RMAT_OF_EULERS(rmat_i2, euler_i);
    DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

    struct Int32Vect3 v4;
    INT32_RMAT_VMULT(v4, rmat_i2, v1);
    DISPLAY_INT32_VECT3("v4", v4);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct FloatVect3 v5;
    VECT3_COPY(v5, v1);
    DISPLAY_FLOAT_VECT3("v5", v5);
    struct FloatVect3 v6;
    FLOAT_QUAT_VMULT(v6, quat_f, v5);
    DISPLAY_FLOAT_VECT3("v6", v6);

}
Esempio n. 5
0
static void test_1(void) {

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

}
void ahrs_update_accel(void) {

  // c2 = ltp z-axis in imu-frame
  struct Int32RMat ltp_to_imu_rmat;
  INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2),
                           RMAT_ELMT(ltp_to_imu_rmat, 1,2),
                           RMAT_ELMT(ltp_to_imu_rmat, 2,2)};
  struct Int32Vect3 residual;

  struct Int32Vect3 pseudo_gravity_measurement;

  if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
    /*
     * centrifugal acceleration in body frame
     * a_c_body = omega x (omega x r)
     * (omega x r) = tangential velocity in body frame
     * a_c_body = omega x vel_tangential_body
     * assumption: tangential velocity only along body x-axis
     */

    // FIXME: check overflows !
#define COMPUTATION_FRAC 16

    const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
    struct Int32Vect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, body_rates_i, vel_tangential_body);
    INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-COMPUTATION_FRAC);

    /* convert centrifucal acceleration from body to imu frame */
    struct Int32Vect3 acc_c_imu;
    INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);

    /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
    INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
  } else {
Esempio n. 7
0
static void test_3(void) {

    /* Compute BODY to IMU eulers */
    struct Int32Eulers b2i_e;
    EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

    /* Compute BODY to IMU quaternion */
    struct Int32Quat b2i_q;
    INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
    //  INT32_QUAT_NORMALIZE(b2i_q);
    //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

    /* Compute BODY to IMU rotation matrix */
    struct Int32RMat b2i_r;
    INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
    //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

    /* Compute LTP to IMU eulers */
    struct Int32Eulers l2i_e;
    EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

    /* Compute LTP to IMU quaternion */
    struct Int32Quat l2i_q;
    INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

    /* Compute LTP to IMU rotation matrix */
    struct Int32RMat l2i_r;
    INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
    //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


    /* again but from quaternion */
    struct Int32RMat l2i_r2;
    INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
    //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

    /* Compute LTP to BODY quaternion */
    struct Int32Quat l2b_q;
    INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

    /* Compute LTP to BODY rotation matrix */
    struct Int32RMat l2b_r;
    INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
    //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

    /* again but from quaternion */
    struct Int32RMat l2b_r2;
    INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
    //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


    /* compute LTP to BODY eulers */
    struct Int32Eulers l2b_e;
    INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

    /* again but from quaternion */
    struct Int32Eulers l2b_e2;
    INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}