/**
* @brief Magnetic field strength in body frame.
* @param[out] data Field strength in microteslas, 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_compass(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
inv_get_compass_set(data, accuracy, timestamp);
if (eMPL_out.compass_status & INV_NEW_DATA)
return 1;
else
return 0;
}
/**
* 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;
}
}
}
