commit 4c5d731f29909f16448fb822644460fd3066ab54
author Meng-hsuan Chung <menghsuan@google.com> Thu Jun 02 22:20:28 2016 -0700
committer Meng-hsuan Chung <menghsuan@google.com> Fri Jun 03 11:14:01 2016 -0700
tree 7571289780ce757664cfe2408a1ceb58ded5d7a3
parent 4bf550b7df8d5530e39d412c520436b58ced5f05

[bmi160] send gyro cal bias to host interface

Bug: 28981599
Bug: 28635583
Change-Id: I43e5620c193410645bac9d6577c930186fe84232

firmware/src/drivers/bosch_bmi160/bosch_bmi160.c

diff --git a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
index 99fe471..5afd391 100644
--- a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
@@ -614,8 +614,8 @@
 {
     { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
             NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
-    { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
-            NANOHUB_INT_NONWAKEUP, 20) },
+    { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
+            NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
 #ifdef MAG_SLAVE_PRESENT
     { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
             NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
@@ -1828,34 +1828,11 @@
                               x, y, z,   //input values
                               &x, &y, &z //calibrated output
                               );
-
-            // Gyro Cal -- Notify HAL about new gyro bias calibration.
-            if (gyroCalNewBiasAvailable(&mTask.gyro_cal)) {
-              // The new calibration value and timestamp:
-              //   mTask.gyro_cal.bias_x;
-              //   mTask.gyro_cal.bias_y;
-              //   mTask.gyro_cal.bias_z;
-              //   mTask.gyro_cal.calibration_time;
-              //
-              // Access gyro cal bias corrections using:
-              //gyroCalGetBias(&mTask.gyro_cal,
-              //               &bias_x, bias_y, &bias_z);
-              INFO_PRINT("TODO: Send updated gyro bias to HAL.");
-            }
 #endif
           }
         }
     }
 
-#ifdef GYRO_CAL_ENABLED
-  #ifdef GYRO_CAL_DBG_ENABLED
-    // Gyro Cal -- Read out Debug data.
-    gyroCalDebugPrint(&mTask.gyro_cal,
-                      &mTask.gyro_debug_state,
-                      rtc_time);
-  #endif
-#endif
-
     if (mSensor->data_evt == NULL) {
         if (!allocateDataEvt(mSensor, rtc_time))
             return;
@@ -1901,6 +1878,28 @@
             return;
     }
 #endif
+#ifdef GYRO_CAL_ENABLED
+    // Gyro Cal -- Notify HAL about new gyro bias calibration
+    if (mSensor->idx == GYR && gyroCalNewBiasAvailable(&mTask.gyro_cal)) {
+        if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
+            // flush existing samples so the bias appears after them
+            flushData(mSensor,
+                    EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[GYR].sensorType));
+            if (!allocateDataEvt(mSensor, rtc_time))
+                return;
+        }
+        mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
+        mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
+        mSensor->data_evt->samples[0].firstSample.biasSample =
+                mSensor->data_evt->samples[0].firstSample.numSamples;
+        sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+        gyroCalGetBias(&mTask.gyro_cal, &sample->x, &sample->y, &sample->z);
+        flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
+
+        if (!allocateDataEvt(mSensor, rtc_time))
+            return;
+    }
+#endif
 
     sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];