Separate gyro calibration and AHRS level functions.

goflying

@@ -1 +1 @@
-Subproject commit 4bbb642cd04e487e65da14a159428eb943ce9290
+Subproject commit 3c8befa0067ddc9ce39b9b2c041fd6450583420a

main/gen_gdl90.go

@@ -1059,6 +1059,7 @@ type settings struct {
 	AHRSLog               bool
 	IMUMapping            [2]int     // Map from aircraft axis to sensor axis: accelerometer
 	SensorQuaternion      [4]float64 // Quaternion mapping from sensor frame to aircraft frame
+	C, D                  [3]float64 // IMU Accel, Gyro zero bias
 	PPM                   int
 	OwnshipModeS          string
 	WatchList             string

main/managementinterface.go

@@ -487,6 +487,21 @@ func handleCageAHRS(w http.ResponseWriter, r *http.Request) {
 	}
 }
 
+func handleCalibrateAHRS(w http.ResponseWriter, r *http.Request) {
+	// define header in support of cross-domain AJAX
+	setNoCache(w)
+	w.Header().Set("Content-Type", "text/plain")
+	w.Header().Set("Access-Control-Allow-Origin", "*")
+	w.Header().Set("Access-Control-Allow-Method", "GET, POST, OPTIONS")
+	w.Header().Set("Access-Control-Allow-Headers", "Origin, X-Requested-With, Content-Type, Accept")
+
+	// For an OPTION method request, we return header without processing.
+	// This ensures we are recognized as supporting cross-domain AJAX REST calls.
+	if r.Method == "POST" {
+		CalibrateAHRS()
+	}
+}
+
 func handleResetGMeter(w http.ResponseWriter, r *http.Request) {
 	// define header in support of cross-domain AJAX
 	setNoCache(w)
@@ -799,6 +814,7 @@ func managementInterface() {
 	http.HandleFunc("/roPartitionRebuild", handleroPartitionRebuild)
 	http.HandleFunc("/develmodetoggle", handleDevelModeToggle)
 	http.HandleFunc("/orientAHRS", handleOrientAHRS)
+	http.HandleFunc("/calibrateAHRS", handleCalibrateAHRS)
 	http.HandleFunc("/cageAHRS", handleCageAHRS)
 	http.HandleFunc("/resetGMeter", handleResetGMeter)
 	http.HandleFunc("/deletelogfile", handleDeleteLogFile)

main/sensors.go

@@ -5,6 +5,7 @@ import (
 	"log"
 	"math"
 	"path/filepath"
+	"strings"
 	"time"
 
 	"../goflying/ahrs"
@@ -24,9 +25,9 @@ var (
 	i2cbus                     embd.I2CBus
 	myPressureReader           sensors.PressureReader
 	myIMUReader                sensors.IMUReader
-	cal                        chan (bool)
+	cal                        chan (string)
 	analysisLogger             *ahrs.AHRSLogger
-	ahrsCalibrating, needsCage bool // Does the sensor orientation matrix need to be recalculated?
+	ahrsCalibrating            bool
 	logMap                     map[string]interface{}
 )
 
@@ -149,7 +150,7 @@ func sensorAttitudeSender() {
 	log.Println("AHRS Info: initializing new Simple AHRS")
 	s := ahrs.NewSimpleAHRS()
 	m := ahrs.NewMeasurement()
-	cal = make(chan (bool), 1)
+	cal = make(chan (string), 1)
 
 	// Set up loggers for analysis
 	ahrswebListener, err := ahrsweb.NewKalmanListener()
@@ -163,28 +164,36 @@ func sensorAttitudeSender() {
 	// Need a sampling freq faster than 10Hz
 	timer := time.NewTicker(50 * time.Millisecond) // ~20Hz update.
 	for {
-		// Do an initial calibration
+		// Set sensor gyro calibrations
-		select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
+		if c, d := &globalSettings.C, &globalSettings.D; d[0]*d[0] + d[1]*d[1] + d[2]*d[2] > 0 {
-		case cal <- true:
+			s.SetCalibrations(c, d)
-		default:
+			log.Printf("AHRS Info: IMU Calibrations read from settings: accel %6f %6f %6f; gyro %6f %6f %6f\n",
+				c[0], c[1], c[2], d[0], d[1], d[2])
+		} else {
+			// Do an initial calibration
+			select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
+			case cal <- "cal":
+			default:
+			}
 		}
 
-		// Set sensor rotation matrix / quaternion
+		// Set sensor quaternion
 		if f := &globalSettings.SensorQuaternion; f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3] > 0 {
 			s.SetSensorQuaternion(f)
-			needsCage = false
 		} else {
-			needsCage = true
+			cal <- "level"
 		}
 
 		failNum = 0
 		<-timer.C
+		time.Sleep(950 * time.Millisecond)
 		for globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected {
 			<-timer.C
 
-			// Calibrate the sensors if requested.
+			// Process calibration and level requests
 			select {
-			case <-cal:
+			case action := <-cal:
+				log.Printf("AHRS Info: cal received action %s\n", action)
 				ahrsCalibrating = true
 				myIMUReader.Read() // Clear out the averages
 				var (
@@ -201,18 +210,21 @@ func sensorAttitudeSender() {
 					if mpuError != nil {
 						log.Printf("AHRS Info: Error reading IMU while calibrating: %s\n", mpuError)
 					} else {
-						s.SetCalibrations(&[3]float64{c1, c2, c3}, &[3]float64{d1, d2, d3})
+						if strings.Contains(action, "cal") { // Calibrate gyros
-						log.Printf("AHRS Info: IMU Calibration values: accel %6f %6f %6f; gyro %6f %6f %6f\n",
+							globalSettings.D = [3]float64{d1, d2, d3}
-							c1, c2, c3, d1, d2, d3)
+							s.SetCalibrations(nil, &globalSettings.D)
-					}
+							log.Printf("AHRS Info: IMU gyro calibration: %3f %3f %3f\n", d1, d2, d3)
-					// Process caging if necessary.
+						}
-					if needsCage {
+						if strings.Contains(action, "level") { // Calibrate accel / level
-						globalSettings.SensorQuaternion = *makeOrientationQuaternion([3]float64{c1, c2, c3})
+							globalSettings.C = [3]float64{c1, c2, c3}
+							s.SetCalibrations(&globalSettings.C, nil)
+							globalSettings.SensorQuaternion = *makeOrientationQuaternion(globalSettings.C)
+							s.SetSensorQuaternion(&globalSettings.SensorQuaternion)
+							s.Reset()
+							log.Printf("AHRS Info: IMU accel calibration: %3f %3f %3f\n", c1, c2, c3)
+							log.Printf("AHRS Info: Caged to quaternion %v\n", globalSettings.SensorQuaternion)
+						}
 						saveSettings()
-						s.SetSensorQuaternion(&globalSettings.SensorQuaternion)
-						s.Reset()
-						needsCage = false
-						log.Println("AHRS Info: Caged")
 					}
 				}
 				ahrsCalibrating = false
@@ -395,8 +407,12 @@ func getMinAccelDirection() (i int, err error) {
 
 // CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
 func CageAHRS() {
-	needsCage = true
+	cal <- "level"
-	cal <- true
+}
+
+// CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
+func CalibrateAHRS() {
+	cal <- "cal"
 }
 
 // ResetAHRSGLoad resets the min and max to the current G load value.

web/js/main.js

@@ -16,6 +16,7 @@ var URL_SHUTDOWN             = "http://" + URL_HOST_BASE + "/shutdown";
 var URL_RESTARTAPP           = "http://" + URL_HOST_BASE + "/restart";
 var URL_DEV_TOGGLE_GET       = "http://" + URL_HOST_BASE + "/develmodetoggle";
 var URL_AHRS_ORIENT          = "http://" + URL_HOST_BASE + "/orientAHRS";
+var URL_AHRS_CAL             = "http://" + URL_HOST_BASE + "/calibrateAHRS";
 var URL_AHRS_CAGE            = "http://" + URL_HOST_BASE + "/cageAHRS";
 var URL_GMETER_RESET         = "http://" + URL_HOST_BASE + "/resetGMeter";
 var URL_DELETELOGFILE        = "http://" + URL_HOST_BASE + "/deletelogfile";

web/plates/js/gps.js

@@ -1,4 +1,4 @@
-angular.module('appControllers').controller('GPSCtrl', GPSCtrl); // get the main module contollers set
+angular.module('appControllers').controller('GPSCtrl', GPSCtrl); // get the main module controllers set
 GPSCtrl.$inject = ['$rootScope', '$scope', '$state', '$http', '$interval']; // Inject my dependencies
 
 // create our controller function with all necessary logic

web/plates/js/settings.js

@@ -240,4 +240,17 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
 			$scope.Ui.turnOn("modalCalibrateFailed");
 		});
 	};
+
+	$scope.calibrateGyros = function() {
+	    console.log("sending calibrate message.");
+	    $http.post(URL_AHRS_CAL).
+            then(function(response) {
+                console.log("Sent calibrate message.");
+        }, function(response) {
+                console.log(response.data);
+                $scope.Calibration_Failure_Message = response.data;
+                $scope.Ui.turnOff("modalCalibrateGyros");
+                $scope.Ui.turnOn("modalCalibrateGyrosFailed");
+        });
+    };
 }

web/plates/settings.html

@@ -81,17 +81,23 @@
             <div class="panel-body">
                 <div class="col-xs-12">
                     <span style="position:relative; overflow: hidden;">
-                        <button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateForward">
+                        <button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateForward"
-                            Set AHRS Sensor Orientation
+                                ng-disabled="!IMU_Sensor_Enabled">Set AHRS Sensor Orientation</button>
-                        </button>
                     </span>
-                    <div class="form-group reset-flow">
+                </div>
-                        <label class="control-label col-xs-5">G Limits</label>
+                <div class="form-group reset-flow">
-                        <form name="GLimitForm" ng-submit="updateGLimits()" novalidate>
+                    <div class="col-xs-12">
-                            <input class="col-xs-7" type="string" required ng-model="GLimits" placeholder="Space-separated negative and positive G meter limits" ng-blur="updateGLimits()" />
+                        <button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateGyros"
-                        </form>
+                                ng-disabled="!IMU_Sensor_Enabled">Calibrate Gyros</button>
                     </div>
                 </div>
+                <div class="form-group reset-flow">
+                    <label class="control-label col-xs-5">G Limits</label>
+                    <form name="GLimitForm" ng-submit="updateGLimits()" novalidate ng-disabled="!IMU_Sensor_Enabled">
+                        <input class="col-xs-7" type="string" required ng-model="GLimits" ng-blur="updateGLimits()"
+                               placeholder="Space-separated negative and positive G meter limits"/>
+                    </form>
+                </div>
             </div>
         </div>
     </div>
@@ -307,7 +313,7 @@
                     <div class="modal-body">
                         <p>There was an error: {{Orientation_Failure_Message}}</p>
                         <p><div id="orientationFailureMessage"></div></p>
-                        <p>The calibration failed.  Please try again.</p>
+                        <p>The orientation failed.  Please try again.</p>
                     </div>
                     <div class="modal-footer">
                         <a ui-turn-off="modalCalibrateFailed" class="btn btn-default btn-primary">OK</a>
@@ -316,4 +322,27 @@
             </div>
         </div>
     </div>
+
+    <div class="modal" ui-if="modalCalibrateGyros" ui-state="modalCalibrateGyros">
+        <div class="modal-overlay "></div>
+        <div class="vertical-alignment-helper center-block">
+            <div class="modal-dialog vertical-align-center">
+                <div class="modal-content">
+                    <div class="modal-header">
+                        <button class="close" ui-turn-off="modalCalibrateGyros"></button>
+                        <h4 class="modal-title">Calibrate MPU Gyros</h4>
+                    </div>
+                    <div class="modal-body">
+                        <p>Press <b>Calibrate</b> and keep the Stratux as stationary as possible for the next second.
+                            You should only do this in calm air as turbulence will throw off the calibrations.</p>
+                    </div>
+                    <div class="modal-footer">
+                        <a ui-turn-off="modalCalibrateGyros" class="btn btn-default">Cancel</a>
+                        <a ng-click="calibrateGyros()" ui-turn-off="modalCalibrateGyros"
+                           class="btn btn-default btn-primary">Calibrate</a>
+                    </div>
+                </div>
+            </div>
+        </div>
+    </div>
 </div>