static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
FLOAT_RMAT_OF_EULERS(fa2c, ea2c);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, ea2c);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
static void test_6(void) {
printf("\n");
struct FloatEulers ea2b;
EULERS_ASSIGN(ea2b, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("ea2b", ea2b);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2b;
FLOAT_RMAT_OF_EULERS(fa2b, ea2b);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp(fa2b, fb2c, 1);
struct FloatQuat qa2b;
FLOAT_QUAT_OF_EULERS(qa2b, ea2b);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp(qa2b, qb2c, 1);
printf("\n");
}
void orientationCalcQuat_f(struct OrientationReps* orientation) {
if (bit_is_set(orientation->status, ORREP_QUAT_F))
return;
if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
}
else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f);
}
else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f);
}
else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
SetBit(orientation->status, ORREP_RMAT_F);
FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f);
}
else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
SetBit(orientation->status, ORREP_EULER_F);
FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f);
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_QUAT_F);
}
float test_quat2(void) {
struct FloatEulers eula2b;
EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b);
struct FloatQuat qa2b;
FLOAT_QUAT_OF_EULERS(qa2b, eula2b);
DISPLAY_FLOAT_QUAT("qa2b", qa2b);
struct DoubleEulers eula2b_d;
EULERS_ASSIGN(eula2b_d, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
struct DoubleQuat qa2b_d;
DOUBLE_QUAT_OF_EULERS(qa2b_d, eula2b_d);
DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d);
struct FloatVect3 u = { 1., 0., 0.};
float angle = RadOfDeg(70.);
struct FloatQuat q;
FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
DISPLAY_FLOAT_QUAT("q ", q);
struct FloatEulers eula2c;
EULERS_ASSIGN(eula2c, RadOfDeg(80.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, eula2c);
DISPLAY_FLOAT_QUAT("qa2c", qa2c);
struct FloatQuat qb2a;
FLOAT_QUAT_INVERT(qb2a, qa2b);
DISPLAY_FLOAT_QUAT("qb2a", qb2a);
struct FloatQuat qb2c1;
FLOAT_QUAT_COMP(qb2c1, qb2a, qa2c);
DISPLAY_FLOAT_QUAT("qb2c1", qb2c1);
struct FloatQuat qb2c2;
FLOAT_QUAT_INV_COMP(qb2c2, qa2b, qa2c);
DISPLAY_FLOAT_QUAT("qb2c2", qb2c2);
return 0.;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
/*
* Initialises our state
*/
FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
const float P0_a = 1.;
const float P0_b = 1e-4;
float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
{ 0., P0_a, 0., 0., 0., 0. },
{ 0., 0., P0_a, 0., 0., 0. },
{ 0., 0., 0., P0_b, 0., 0. },
{ 0., 0., 0., 0., P0_b, 0. },
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
}
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
}
static void test_10(void) {
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
FLOAT_QUAT_OF_EULERS(quat, euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
INT32_QUAT_OF_EULERS(quati, euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
INT32_RMAT_OF_EULERS(rmati, euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
INT32_QUAT_ZERO( body_to_imu_quat);
INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
static inline void compute_ahrs_representations(void) {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
void imu_float_init(void) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
EULERS_ASSIGN(imuf.body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers);
FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers);
}
static void traj_coordinated_circle_init(void) {
aos.traj->te = 120.;
const float speed = 15; // m/s
const float R = 100; // radius in m
// tan phi = v^2/Rg
float phi = atan2(speed*speed, R*9.81);
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
}
static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate);
#else
struct FloatEulers eul_f;
EULERS_FLOAT_OF_BFP(eul_f, ahrs_impl.ltp_to_imu_euler);
FLOAT_QUAT_OF_EULERS(output[i].quat_est, eul_f);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
// memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
void update_ahrs_from_sim(void) {
ahrs_float.ltp_to_imu_euler.phi = sim_phi;
ahrs_float.ltp_to_imu_euler.theta = sim_theta;
ahrs_float.ltp_to_imu_euler.psi = sim_psi;
ahrs_float.imu_rate.p = sim_p;
ahrs_float.imu_rate.q = sim_q;
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
}
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);
}
static void test_5(void) {
struct FloatEulers fe;
EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
printf("\n");
struct FloatQuat fq;
FLOAT_QUAT_OF_EULERS(fq, fe);
test_eulers_of_quat(fq, 1);
printf("\n");
struct FloatRMat frm;
FLOAT_RMAT_OF_EULERS(frm, fe);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
test_eulers_of_rmat(frm, 1);
printf("\n");
}
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_update_fw_estimator( void )
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
// export results to estimator
estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_body_euler.psi;
estimator_p = ahrs_float.body_rate.p;
estimator_q = ahrs_float.body_rate.q;
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
void imu_float_init(struct ImuFloat* imuf) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
EULERS_ASSIGN(imuf->body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
#endif
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void compute_body_orientation_and_rates(void) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
/* Set state */
#ifdef AHRS_UPDATE_FW_ESTIMATOR
struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. };
struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat;
FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat);
FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
stateSetNedToBodyQuat_f(<p_to_neutrals_quat);
#else
stateSetNedToBodyQuat_f(<p_to_body_quat);
#endif
/* compute body rates */
struct FloatRates body_rate;
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
stateSetBodyRates_f(&body_rate);
}
float test_quat_of_rmat(void) {
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatMat33 rm;
FLOAT_RMAT_OF_EULERS(rm, eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
FLOAT_QUAT_OF_RMAT(q, rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
FLOAT_QUAT_OF_EULERS(qref, eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatMat33 r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
FLOAT_RMAT_OF_EULERS_312(r_att, e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
FLOAT_QUAT_OF_RMAT(q_att, r_att);
struct FloatEulers e_att;
FLOAT_EULERS_OF_RMAT(e_att, r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
FLOAT_EULERS_OF_QUAT(e_att, q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 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);
}
static void test_4_float(void) {
printf("euler to quat to euler - float\n");
/* initial euler angles */
struct FloatEulers e;
EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("e", e);
/* transform to quaternion */
struct FloatQuat q;
FLOAT_QUAT_OF_EULERS(q, e);
// DISPLAY_FLOAT_QUAT("q", q);
FLOAT_QUAT_NORMALIZE(q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
struct FloatEulers e2;
FLOAT_EULERS_OF_QUAT(e2, q);
DISPLAY_FLOAT_EULERS_DEG("e2", e2);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
static void traj_sineX_quad_update(void) {
const float om = RadOfDeg(10);
if ( aos.time > (M_PI/om) ) {
const float a = 20;
struct FloatVect3 jerk;
VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0);
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
0. };
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
static void traj_coordinated_circle_update(void) {
const float speed = 15; // m/s
const float R = 100; // radius in m
float omega = speed / R;
// tan phi = v^2/Rg
float phi = atan2(speed*speed, R*9.81);
if ( aos.time > 2.*M_PI/omega) {
VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.);
// float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI;
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
struct FloatEulers e_dot;
EULERS_ASSIGN(e_dot, 0., 0., omega);
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
static void traj_stop_stop_x_update(void){
const float t0 = 5.;
const float dt_jerk = 0.75;
const float dt_nojerk = 10.;
const float val_jerk = 5.;
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);
if (aos.time < t0) return;
else if (aos.time < t0+dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
0. };
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
static void traj_step_phi_update(void) {
if (aos.time > 5) {
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
}
}
void aos_init(int traj_nb) {
aos.traj = &traj[traj_nb];
aos.time = 0;
aos.dt = 1./AHRS_PROPAGATE_FREQUENCY;
aos.traj->ts = 0;
aos.traj->ts = 1.; // default to one second
/* default state */
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_VECT3_ZERO(aos.ltp_pos);
FLOAT_VECT3_ZERO(aos.ltp_vel);
FLOAT_VECT3_ZERO(aos.ltp_accel);
FLOAT_VECT3_ZERO(aos.ltp_jerk);
aos.traj->init_fun();
imu_init();
ahrs_init();
ahrs_aligner_init();
#ifdef PERFECT_SENSORS
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
#else
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
#endif
#ifdef FORCE_ALIGNEMENT
// DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
aos_compute_sensors();
// DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
// DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
// DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
ahrs_align();
// DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);
#endif
#ifdef DISABLE_ALIGNEMENT
printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
printf("# DISABLE_MAG_UPDATE\n");
#endif
printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]);
printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
printf("# PERFECT_SENSORS\n");
#endif
printf("# tajectory : %s\n", aos.traj->name);
};
