From a9be582f90953b351d2a0ac0e62197b27fa39a88 Mon Sep 17 00:00:00 2001
From: Peli de Halleux <pelikhan@users.noreply.github.com>
Date: Thu, 4 Jan 2018 23:21:19 -0800
Subject: [PATCH] gyro calibration done right (#186)

---
 libs/core/input.ts                            |  4 ++
 .../_locales/gyro-sensor-jsdoc-strings.json   |  3 +-
 .../_locales/gyro-sensor-strings.json         |  3 +-
 libs/gyro-sensor/gyro.ts                      | 50 +++++++++++++++++--
 4 files changed, 53 insertions(+), 7 deletions(-)

diff --git a/libs/core/input.ts b/libs/core/input.ts
index 88931bc3..eb9f03be 100644
--- a/libs/core/input.ts
+++ b/libs/core/input.ts
@@ -309,6 +309,10 @@ namespace sensors.internal {
                 return 0
             return getUartNumber(fmt, off, this._port)
         }
+
+        protected reset() {
+            if (this.isActive()) uartReset(this._port);
+        }
     }
 
     function uartReset(port: number) {
diff --git a/libs/gyro-sensor/_locales/gyro-sensor-jsdoc-strings.json b/libs/gyro-sensor/_locales/gyro-sensor-jsdoc-strings.json
index d8048229..c8d334f5 100644
--- a/libs/gyro-sensor/_locales/gyro-sensor-jsdoc-strings.json
+++ b/libs/gyro-sensor/_locales/gyro-sensor-jsdoc-strings.json
@@ -1,4 +1,5 @@
 {
   "sensors.GyroSensor.angle": "Get the current angle from the gyroscope.",
-  "sensors.GyroSensor.rate": "Get the current rotation rate from the gyroscope."
+  "sensors.GyroSensor.calibrate": "Forces a calibration of the gyro. Must be called when the sensor is completely still.",
+  "sensors.GyroSensor.rotationRate": "Get the current rotation rate from the gyroscope."
 }
\ No newline at end of file
diff --git a/libs/gyro-sensor/_locales/gyro-sensor-strings.json b/libs/gyro-sensor/_locales/gyro-sensor-strings.json
index baa3610b..127b6e61 100644
--- a/libs/gyro-sensor/_locales/gyro-sensor-strings.json
+++ b/libs/gyro-sensor/_locales/gyro-sensor-strings.json
@@ -1,6 +1,7 @@
 {
   "sensors.GyroSensor.angle|block": "%sensor|angle",
-  "sensors.GyroSensor.rate|block": "%sensor|rotation rate",
+  "sensors.GyroSensor.calibrate|block": "%sensor|calibrate",
+  "sensors.GyroSensor.rotationRate|block": "%sensor|rotation rate",
   "sensors.gyro1|block": "gyro 1",
   "sensors.gyro2|block": "gyro 2",
   "sensors.gyro3|block": "gyro 3",
diff --git a/libs/gyro-sensor/gyro.ts b/libs/gyro-sensor/gyro.ts
index d3adfa2a..4b169792 100644
--- a/libs/gyro-sensor/gyro.ts
+++ b/libs/gyro-sensor/gyro.ts
@@ -7,8 +7,10 @@ const enum GyroSensorMode {
 namespace sensors {
     //% fixedInstances
     export class GyroSensor extends internal.UartSensor {
+        private calibrating: boolean;
         constructor(port: number) {
             super(port)
+            this.calibrating = false;
         }
 
         _deviceType() {
@@ -32,6 +34,9 @@ namespace sensors {
         //% weight=65 blockGap=8
         //% group="Gyro Sensor"
         angle(): number {
+            if (this.calibrating)
+                pauseUntil(() => !this.calibrating, 2000);
+
             this.setMode(GyroSensorMode.Angle)
             return this.getNumber(NumberFormat.Int16LE, 0)
         }
@@ -40,7 +45,7 @@ namespace sensors {
          * Get the current rotation rate from the gyroscope.
          * @param sensor the gyroscope to query the request
          */
-        //% help=input/gyro/rate
+        //% help=input/gyro/rotation-rate
         //% block="%sensor|rotation rate"
         //% blockId=gyroGetRate
         //% parts="gyroscope"
@@ -48,18 +53,53 @@ namespace sensors {
         //% sensor.fieldEditor="ports"
         //% weight=65 blockGap=8        
         //% group="Gyro Sensor"
-        rate(): number {
+        rotationRate(): number {
+            if (this.calibrating)
+                pauseUntil(() => !this.calibrating, 2000);
+
             this.setMode(GyroSensorMode.Rate)
             return this.getNumber(NumberFormat.Int16LE, 0)
         }
-    }
 
-    //% fixedInstance whenUsed block="gyro 1" jres=icons.port1
-    export const gyro1: GyroSensor = new GyroSensor(1)
+        /**
+         * Forces a calibration of the gyro. Must be called when the sensor is completely still.
+         */
+        //% help=input/gyro/calibrate
+        //% block="%sensor|calibrate"
+        //% blockId=gyroCalibrate   
+        //% parts="gyroscope"
+        //% blockNamespace=sensors
+        //% sensor.fieldEditor="ports"
+        //% weight=65 blockGap=8        
+        //% group="Gyro Sensor"
+        calibrate(): void {
+            if (this.calibrating) return; // already in calibration mode
+
+            this.calibrating = true;
+            // may be triggered by a button click, give time to settle
+            loops.pause(500);
+            // send a reset command
+            this.reset();
+            // we need to switch mode twice to perform a calibration
+            if (this.mode == GyroSensorMode.Rate)
+                this.setMode(GyroSensorMode.Angle);
+            else
+                this.setMode(GyroSensorMode.Rate);
+            // switch back and wait
+            if (this.mode == GyroSensorMode.Rate)
+                this.setMode(GyroSensorMode.Angle);
+            else
+                this.setMode(GyroSensorMode.Rate);
+            this.calibrating = false;
+        }
+    }
 
     //% fixedInstance whenUsed block="gyro 2" weight=95 jres=icons.port2
     export const gyro2: GyroSensor = new GyroSensor(2)
 
+    //% fixedInstance whenUsed block="gyro 1" jres=icons.port1
+    export const gyro1: GyroSensor = new GyroSensor(1)
+
     //% fixedInstance whenUsed block="gyro 3" jres=icons.port3
     export const gyro3: GyroSensor = new GyroSensor(3)