GMINL-3234: Clamp very low gyro reading to zero

This avoids accumulating error over long periods of time.
Incidently, be more cautious with the gyro status field,
as it's not valid for the uncal gyro.

Tracked-On: https://jira01.devtools.intel.com/browse/GMINL-3234
Change-Id: Ic4fa964bb31c3e223eabf23382be27f51e3df529
Signed-off-by: Patrick Porlan <patrick.porlan@intel.com>

diff --git a/gyro-calibration.c b/gyro-calibration.c
index b6defec..c71ee3b 100644 (file)
--- a/gyro-calibration.c
+++ b/gyro-calibration.c
@@ -129,15 +129,15 @@ void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
                info->cal_level = gyro_collect(event->data[0], event->data[1],
                                               event->data[2], cal_data);
 
-       if (info->cal_level)
-               event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
-
        switch (event->type) {
                case SENSOR_TYPE_GYROSCOPE:
                        /* For the gyroscope apply the bias */
                        event->data[0] = event->data[0] - cal_data->bias_x;
                        event->data[1] = event->data[1] - cal_data->bias_y;
                        event->data[2] = event->data[2] - cal_data->bias_z;
+
+                       if (info->cal_level)
+                              event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
                        break;
 
                case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:

diff --git a/transform.c b/transform.c
index a49300d..b68f316 100644 (file)
--- a/transform.c
+++ b/transform.c
@@ -261,6 +261,62 @@ static void denoise (struct sensor_info_t* si, struct sensors_event_t* data,
 }
 
 
+static void clamp_gyro_readings_to_zero (int s, struct sensors_event_t* data)
+{
+       float x, y, z;
+       float near_zero;
+
+       switch (sensor_info[s].type) {
+               case SENSOR_TYPE_GYROSCOPE:
+                       x = data->data[0];
+                       y = data->data[1];
+                       z = data->data[2];
+                       break;
+
+               case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+                       x = data->data[0] - data->uncalibrated_gyro.bias[0];
+                       y = data->data[1] - data->uncalibrated_gyro.bias[1];
+                       z = data->data[2] - data->uncalibrated_gyro.bias[2];
+                       break;
+
+               default:
+                       return;
+       }
+
+       /* If we're calibrated, don't filter out as much */
+       if (sensor_info[s].cal_level > 0)
+               near_zero = 0.02; /* rad/s */
+       else
+               near_zero = 0.1;
+
+       /* If motion on all axes is small enough */
+       if (fabs(x) < near_zero && fabs(y) < near_zero && fabs(z) < near_zero) {
+
+               /*
+                * Report that we're not moving at all... but not exactly zero
+                * as composite sensors (orientation, rotation vector) don't
+                * seem to react very well to it.
+                */
+               switch (sensor_info[s].type) {
+                       case SENSOR_TYPE_GYROSCOPE:
+                               data->data[0] *= 0.000001;
+                               data->data[1] *= 0.000001;
+                               data->data[2] *= 0.000001;
+                               break;
+
+                       case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+                               data->data[0]= data->uncalibrated_gyro.bias[0]
+                                               + 0.000001 * x;
+                               data->data[1]= data->uncalibrated_gyro.bias[1]
+                                               + 0.000001 * y;
+                               data->data[2]= data->uncalibrated_gyro.bias[2]
+                                               + 0.000001 * z;
+                               break;
+               }
+       }
+}
+
+
 static int finalize_sample_default (int s, struct sensors_event_t* data)
 {
        /* Swap fields if we have a custom channel ordering on this sensor */
@@ -283,7 +339,7 @@ static int finalize_sample_default (int s, struct sensors_event_t* data)
                        break;
 
                case SENSOR_TYPE_GYROSCOPE:
-               case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+
                        /*
                         * Report medium accuracy by default ; higher accuracy
                         * levels will be reported once, and if, we achieve
@@ -291,6 +347,10 @@ static int finalize_sample_default (int s, struct sensors_event_t* data)
                         */
                        data->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM;
 
+                       /* ... fall through */
+
+               case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+
                        /*
                         * We're only trying to calibrate data from continuously
                         * firing gyroscope drivers, as motion based ones use
@@ -313,6 +373,9 @@ static int finalize_sample_default (int s, struct sensors_event_t* data)
                                        sensor_info[s].event_count < MIN_SAMPLES)
                                                return 0;
                        }
+
+                       /* Clamp near zero moves to (0,0,0) if appropriate */
+                       clamp_gyro_readings_to_zero(s, data);
                        break;
 
                case SENSOR_TYPE_LIGHT: