static void send_att(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat);
struct Int32Eulers eulers_imu;
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&eulers_imu.phi,
&eulers_imu.theta,
&eulers_imu.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
&ahrs_finv_id);
}
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);
}
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 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_identity(&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);
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
struct Int32Eulers eul321_i;
EULERS_BFP_OF_REAL(eul321_i, (*eul_f));
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); }
struct FloatRMat rmat_f;
float_rmat_of_eulers_321(&rmat_f, eul_f);
if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); }
if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); }
struct Int32RMat rmat_i;
int32_rmat_of_eulers_321(&rmat_i, &eul321_i);
if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); }
struct FloatQuat qf;
float_quat_of_rmat(&qf, &rmat_f);
//FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) { DISPLAY_FLOAT_QUAT("qf", qf); }
struct Int32Quat qi;
int32_quat_of_rmat(&qi, &rmat_i);
//int32_quat_wrap_shortest(&qi);
if (display) { DISPLAY_INT32_QUAT("qi", qi); }
if (display) { DISPLAY_INT32_QUAT_2("qi", qi); }
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
// dot product of two quaternions is 1 if they represent same rotation
float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
float err_norm = fabs(fabs(qi_dot_qf) - 1.);
if (display) { printf("err %f\n", err_norm); }
if (display) { printf("\n"); }
return err_norm;
}
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);
}
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_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 send_att(void) {
struct FloatEulers ltp_to_imu_euler;
FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
static void send_att(struct transport_tx *trans, struct link_device *dev) {
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
float test_rmat_of_eulers_312(void) {
struct FloatEulers eul312_f;
EULERS_ASSIGN(eul312_f, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("eul312_f", eul312_f);
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, eul312_f);
DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, eul312_f);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
return 0;
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);
struct FloatQuat qf;
FLOAT_QUAT_OF_RMAT(qf, rmat_f);
FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) DISPLAY_FLOAT_QUAT("qf", qf);
struct Int32Quat qi;
INT32_QUAT_OF_RMAT(qi, rmat_i);
INT32_QUAT_WRAP_SHORTEST(qi);
if (display) DISPLAY_INT32_QUAT("qi", qi);
if (display) DISPLAY_INT32_QUAT_2("qi", qi);
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
struct FloatQuat qerr;
QUAT_DIFF(qerr, qif, qf);
float err_norm = FLOAT_QUAT_NORM(qerr);
if (display) printf("err %f\n", err_norm);
if (display) printf("\n");
return err_norm;
}
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 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);
}
void UM6_packet_read_message(void)
{
if ((UM6_status == UM6Running) && PacketLength > 11) {
switch (PacketAddr) {
case IMU_UM6_GYRO_PROC:
UM6_rate.p =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
UM6_rate.q =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
UM6_rate.r =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
break;
case IMU_UM6_ACCEL_PROC:
UM6_accel.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
UM6_accel.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
UM6_accel.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
break;
case IMU_UM6_MAG_PROC:
UM6_mag.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
UM6_mag.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
UM6_mag.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
// Assume the same units for magnetic field
// Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
break;
case IMU_UM6_EULER:
UM6_eulers.phi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
UM6_eulers.theta =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
UM6_eulers.psi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
break;
case IMU_UM6_QUAT:
UM6_quat.qi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
UM6_quat.qx =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
UM6_quat.qy =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
UM6_quat.qz =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
break;
default:
break;
}
}
}