/**
* Magnetic heading/field strength in body frame.
* TODO: No difference between mag_north and true_north yet.
* @param[out] mag_north Heading relative to magnetic north in degrees.
* @param[out] true_north Heading relative to true north in degrees.
* @param[out] values Field strength in milligauss.
* @param[out] accuracy 0 (uncalibrated) to 3 (most accurate).
* @param[out] timestamp Time when sensor was sampled.
*/
void inv_get_sensor_type_compass_float(float *mag_north, float *true_north,
float *values, int8_t *accuracy, inv_time_t *timestamp)
{
long compass[3];
long q00, q12, q22, q03, t1, t2;
/* 1 uT = 10 milligauss. */
#define COMPASS_CONVERSION (10 / 65536.f)
inv_get_compass_set(compass, accuracy, timestamp);
if (values) {
values[0] = (float)compass[0]*COMPASS_CONVERSION;
values[1] = (float)compass[1]*COMPASS_CONVERSION;
values[2] = (float)compass[2]*COMPASS_CONVERSION;
}
/* TODO: Stolen from euler angle computation. Calculate this only once per
* callback.
*/
q00 = inv_q29_mult(dl_out.quat[0], dl_out.quat[0]);
q12 = inv_q29_mult(dl_out.quat[1], dl_out.quat[2]);
q22 = inv_q29_mult(dl_out.quat[2], dl_out.quat[2]);
q03 = inv_q29_mult(dl_out.quat[0], dl_out.quat[3]);
t1 = q12 - q03;
t2 = q22 + q00 - (1L << 30);
if (mag_north) {
*mag_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
if (*mag_north < 0)
*mag_north += 360;
}
if (true_north) {
if (!mag_north) {
*true_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
if (*true_north < 0)
*true_north += 360;
} else {
*true_north = *mag_north;
}
}
}
/**
* @brief Quaternion-derived heading.
* @param[out] data Heading in degrees, q16 fixed point.
* @param[out] accuracy Accuracy of the measurement from 0 (least accurate)
* to 3 (most accurate).
* @param[out] timestamp The time in milliseconds when this sensor was read.
* @return 1 if data was updated.
*/
int inv_get_sensor_type_heading(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
long t1, t2, q00, q03, q12, q22;
float fdata;
q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]);
q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]);
q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]);
q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]);
/* X component of the Ybody axis in World frame */
t1 = q12 - q03;
/* Y component of the Ybody axis in World frame */
t2 = q22 + q00 - (1L << 30);
fdata = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
if (fdata < 0.f)
fdata += 360.f;
data[0] = (long)(fdata * 65536.f);
accuracy[0] = eMPL_out.quat_accuracy;
timestamp[0] = eMPL_out.nine_axis_timestamp;
return eMPL_out.nine_axis_status;
}
inv_error_t inv_get_gravity_6x(long *data)
{
data[0] =
inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]);
data[1] =
inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]);
data[2] =
(inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) -
1073741824L;
return INV_SUCCESS;
}
/**
* @brief Body-to-world frame euler angles.
* The euler angles are output with the following convention:
* Pitch: -180 to 180
* Roll: -90 to 90
* Yaw: -180 to 180
* @param[out] data Euler angles in degrees, q16 fixed point.
* @param[out] accuracy Accuracy of the measurement from 0 (least accurate)
* to 3 (most accurate).
* @param[out] timestamp The time in milliseconds when this sensor was read.
* @return 1 if data was updated.
*/
int inv_get_sensor_type_euler(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
long t1, t2, t3;
long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
float values[3];
q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]);
q01 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[1]);
q02 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[2]);
q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]);
q11 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[1]);
q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]);
q13 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[3]);
q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]);
q23 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[3]);
q33 = inv_q29_mult(eMPL_out.quat[3], eMPL_out.quat[3]);
/* X component of the Ybody axis in World frame */
t1 = q12 - q03;
/* Y component of the Ybody axis in World frame */
t2 = q22 + q00 - (1L << 30);
values[2] = -atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
/* Z component of the Ybody axis in World frame */
t3 = q23 + q01;
values[0] =
atan2f((float) t3,
sqrtf((float) t1 * t1 +
(float) t2 * t2)) * 180.f / (float) M_PI;
/* Z component of the Zbody axis in World frame */
t2 = q33 + q00 - (1L << 30);
if (t2 < 0) {
if (values[0] >= 0)
values[0] = 180.f - values[0];
else
values[0] = -180.f - values[0];
}
/* X component of the Xbody axis in World frame */
t1 = q11 + q00 - (1L << 30);
/* Y component of the Xbody axis in World frame */
t2 = q12 + q03;
/* Z component of the Xbody axis in World frame */
t3 = q13 - q02;
values[1] =
(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
180.f / (float) M_PI - 90);
if (values[1] >= 90)
values[1] = 180 - values[1];
if (values[1] < -90)
values[1] = -180 - values[1];
data[0] = (long)(values[0] * 65536.f);
data[1] = (long)(values[1] * 65536.f);
data[2] = (long)(values[2] * 65536.f);
accuracy[0] = eMPL_out.quat_accuracy;
timestamp[0] = eMPL_out.nine_axis_timestamp;
return eMPL_out.nine_axis_status;
}
