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); }
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); }
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_icq.accel_cnt == 0) { return; } // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.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_icq.correct_gravity && ahrs_icq.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 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); struct Int32Rates body_rate; int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu); } else {
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); }
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); }