From d214a452e02b69cc55cba5a9445aa1aca7613bc5 Mon Sep 17 00:00:00 2001 From: Patrick Porlan Date: Wed, 14 May 2014 18:54:36 +0200 Subject: [PATCH] STPK-1429 Fix coordinate system mapping for gyro and magn It turns out they are consistent. Android uses a different coordinate system, with axes based on the "natural orientation" of the device, but the transfom is the same for these three sensors as they are using the same convention. Issue: STPK-1429 Change-Id: I5e9b819a30422ca8134c39e427c21178cdef68a0 Signed-off-by: Patrick Porlan --- transform.c | 37 ++++++++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/transform.c b/transform.c index b91642c..4ead4e3 100644 --- a/transform.c +++ b/transform.c @@ -173,6 +173,7 @@ static void finalize_sample_default(int s, struct sensors_event_t* data) { int i = sensor_info[s].catalog_index; int sensor_type = sensor_catalog[i].type; + float x, y, z; switch (sensor_type) { case SENSOR_TYPE_ACCELEROMETER: @@ -181,19 +182,37 @@ static void finalize_sample_default(int s, struct sensors_event_t* data) * /hardware/libhardware/include/hardware/sensors.h * for a discussion of what Android expects */ - data->data[0] = -data->data[0]; - data->data[2] = -data->data[2]; + x = -data->data[0]; + y = data->data[1]; + z = -data->data[2]; + + data->data[0] = x; + data->data[1] = y; + data->data[2] = z; + break; + + case SENSOR_TYPE_MAGNETIC_FIELD: + x = -data->data[0]; + y = data->data[1]; + z = -data->data[2]; + + data->data[0] = x; + data->data[1] = y; + data->data[2] = z; break; case SENSOR_TYPE_GYROSCOPE: + x = -data->data[0]; + y = data->data[1]; + z = -data->data[2]; + /* Limit drift */ - if ( fabs(data->data[0]) < 0.1 && - fabs(data->data[1]) < 0.1 && - fabs(data->data[2]) < 0.1) { - data->data[0] = 0; - data->data[1] = 0; - data->data[2] = 0; - } + if (fabs(x) < 0.1 && fabs(y) < 0.1 && fabs(z) < 0.1) + x = y = z = 0; + + data->data[0] = x; + data->data[1] = y; + data->data[2] = z; break; case SENSOR_TYPE_AMBIENT_TEMPERATURE: -- 2.11.0