From f2cfe3344b8379f05fa115e876b174f607bfd948 Mon Sep 17 00:00:00 2001
From: Eric Westphal <BonanzaEric@gmail.com>
Date: Thu, 16 Feb 2017 16:57:30 -0500
Subject: [PATCH] Re-do sensor orientation setting method.

---
 main/gen_gdl90.go           |  4 ++--
 main/managementinterface.go | 11 +++++------
 main/sensors.go             | 23 +++++++++++++++--------
 3 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/main/gen_gdl90.go b/main/gen_gdl90.go
index 86fdf261..8233c8f6 100644
--- a/main/gen_gdl90.go
+++ b/main/gen_gdl90.go
@@ -1039,7 +1039,7 @@ type settings struct {
 	DisplayTrafficSource bool
 	DEBUG                bool
 	ReplayLog            bool
-	IMUMapping           [3]int // Map from aircraft axis to sensor axis: accelerometer
+	IMUMapping           [2]int // Map from aircraft axis to sensor axis: accelerometer
 	PPM                  int
 	OwnshipModeS         string
 	WatchList            string
@@ -1108,7 +1108,7 @@ func defaultSettings() {
 	globalSettings.DEBUG = false
 	globalSettings.DisplayTrafficSource = false
 	globalSettings.ReplayLog = false //TODO: 'true' for debug builds.
-	globalSettings.IMUMapping = [3]int{2, -1, 3} // RY83XAI orientation mapping
+	globalSettings.IMUMapping = [2]int{-1, -3} // OpenFlightBox AHRS normal mapping
 	globalSettings.OwnshipModeS = "F00000"
 	globalSettings.DeveloperMode = false
 }
diff --git a/main/managementinterface.go b/main/managementinterface.go
index 96ee035e..1d01e4d4 100644
--- a/main/managementinterface.go
+++ b/main/managementinterface.go
@@ -286,8 +286,8 @@ func handleSettingsSetRequest(w http.ResponseWriter, r *http.Request) {
 							globalSettings.ReplayLog = v
 						}
 					case "IMUMapping":
-						if globalSettings.IMUMapping != val.([3]int) {
-							globalSettings.IMUMapping = val.([3]int)
+						if globalSettings.IMUMapping != val.([2]int) {
+							globalSettings.IMUMapping = val.([2]int)
 							globalStatus.IMUConnected = false // Force a restart of the IMU reader
 						}
 					case "PPM":
@@ -384,7 +384,7 @@ func handleOrientAHRS(w http.ResponseWriter, r *http.Request) {
 	if r.Method == "POST" {
 		var (
 			action []byte = make([]byte, 1)
-			u, l int
+			u int
 			err error
 		)
 
@@ -415,11 +415,10 @@ func handleOrientAHRS(w http.ResponseWriter, r *http.Request) {
 				return
 			}
 
-			l = 6 / f / u
-			globalSettings.IMUMapping = [3]int{f, l, u}
+			globalSettings.IMUMapping = [2]int{f, u}
 			saveSettings()
 			globalStatus.IMUConnected = false // restart the processes depending on the orientation
-			log.Printf("AHRS Info: sensor orientation success! forward: %d; left: %d; up: %d\n", f, l, u)
+			log.Printf("AHRS Info: sensor orientation success! forward: %d; up: %d\n", f, u)
 		default: // Cancel the sensor calibration.
 			log.Println("AHRS Info: sensor orientation: canceled")
 		}
diff --git a/main/sensors.go b/main/sensors.go
index 6f250988..42a93703 100644
--- a/main/sensors.go
+++ b/main/sensors.go
@@ -181,19 +181,26 @@ func sensorAttitudeSender() {
 	for {
 		if globalSettings.IMUMapping[0]==0 { // if unset, default to RY836AI
 			globalSettings.IMUMapping[0] = -1 // +2
-			globalSettings.IMUMapping[1] = +2 // -1
-			globalSettings.IMUMapping[2] = -3 // +3
+			globalSettings.IMUMapping[1] = -3 // +3
 			saveSettings()
 		}
 		f := globalSettings.IMUMapping
+
+		// Set up orientation matrix; a bit ugly for now
 		ff = new([3][3]float64)
-		for i := 0; i < 3; i++ {
-			if f[i] < 0 {
-				ff[i][-f[i] - 1] = -1
-			} else {
-				ff[i][f[i] - 1] = +1
-			}
+		if f[0] < 0 {
+			ff[0][-f[0] - 1] = -1
+		} else {
+			ff[0][+f[0] - 1] = +1
 		}
+		if f[1] < 0 {
+			ff[2][-f[1] - 1] = -1
+		} else {
+			ff[2][+f[1] - 1] = +1
+		}
+		ff[1][0] = ff[2][1] * ff[0][2] - ff[2][2] * ff[0][1]
+		ff[1][1] = ff[2][2] * ff[0][0] - ff[2][0] * ff[0][2]
+		ff[1][2] = ff[2][0] * ff[0][1] - ff[2][1] * ff[0][0]
 
 		failnum = 0
 		<-timer.C