static void send_euler(struct transport_tx *trans, struct link_device *dev) {
struct Int32Eulers ltp_to_imu_euler;
int32_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
<p_to_imu_euler.phi,
<p_to_imu_euler.theta,
<p_to_imu_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi));
}
void orientationCalcEulers_i(struct OrientationReps* orientation)
{
if (bit_is_set(orientation->status, ORREP_EULER_I)) {
return;
}
if (bit_is_set(orientation->status, ORREP_EULER_F)) {
EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
} else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
} else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
} else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
SetBit(orientation->status, ORREP_RMAT_I);
int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_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_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_EULER_I);
}
float test_eulers_of_quat(struct FloatQuat fq, int display)
{
struct FloatEulers fe;
float_eulers_of_quat(&fe, &fq);
struct Int32Quat iq;
QUAT_BFP_OF_REAL(iq, fq);
struct Int32Eulers ie;
int32_eulers_of_quat(&ie, &iq);
struct FloatEulers fe2;
EULERS_FLOAT_OF_BFP(fe2, ie);
EULERS_SUB(fe2, ie);
float norm_err = FLOAT_EULERS_NORM(fe2);
if (display) {
printf("euler of quat\n");
DISPLAY_FLOAT_QUAT("fq", fq);
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
DISPLAY_INT32_EULERS("ie", ie);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
}
return norm_err;
}
static void test_4_int(void)
{
printf("euler to quat to euler - int\n");
/* initial euler angles */
struct Int32Eulers _e;
EULERS_ASSIGN(_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("euler orig ", _e);
/* transform to quaternion */
struct Int32Quat _q;
int32_quat_of_eulers(&_q, &_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q);
// int32_quat_normalize(&_q);
// DISPLAY_INT32_QUAT_2("_q_n", _q);
/* back to eulers */
struct Int32Eulers _e2;
int32_eulers_of_quat(&_e2, &_q);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2);
}
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);
}
static void report(void) {
int output_sensors = FALSE;
int output_pos = FALSE;
printf("%f ", aos.time);
printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
printf("%f %f %f ", DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r));
printf("%f %f %f ", DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
#if AHRS_TYPE == AHRS_TYPE_ICQ
struct Int32Eulers ltp_to_imu_euler_i;
int32_eulers_of_quat(<p_to_imu_euler_i, &ahrs_impl.ltp_to_imu_quat);
struct FloatEulers ltp_to_imu_euler_f;
EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i);
printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi),
DegOfRad(ltp_to_imu_euler_f.theta),
DegOfRad(ltp_to_imu_euler_f.psi));
struct FloatRates imu_rate_f;
RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate);
printf("%f %f %f ", DegOfRad(imu_rate_f.p),
DegOfRad(imu_rate_f.q),
DegOfRad(imu_rate_f.r));
struct FloatRates bias;
RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
printf("%f %f %f ", DegOfRad(bias.p),
DegOfRad(bias.q),
DegOfRad(bias.r));
#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR
printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi),
DegOfRad(ahrs_impl.ltp_to_imu_euler.theta),
DegOfRad(ahrs_impl.ltp_to_imu_euler.psi));
printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p),
DegOfRad(ahrs_impl.imu_rate.q),
DegOfRad(ahrs_impl.imu_rate.r));
printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
DegOfRad(ahrs_impl.gyro_bias.q),
DegOfRad(ahrs_impl.gyro_bias.r));
#endif
if (output_pos) {
printf("%f %f %f ", aos.ltp_pos.x,
aos.ltp_pos.y,
aos.ltp_pos.z);
}
if (output_sensors) {
}
printf("\n");
}
