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);
}
