From 9f2e80029c19e0057551cc0d3cc151a6fd9ba473 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 28 Sep 2019 20:26:58 +0200
Subject: [PATCH 01/21] dzukuvx-bno055-secondary-imu

---
 make/source.mk                            |  2 +
 src/main/build/debug.h                    |  1 +
 src/main/drivers/accgyro/accgyro_bno055.c | 77 +++++++++++++++++++++++
 src/main/drivers/accgyro/accgyro_bno055.h | 25 ++++++++
 src/main/drivers/bus.h                    |  1 +
 src/main/fc/fc_core.c                     | 15 +++++
 src/main/fc/fc_core.h                     |  1 +
 src/main/fc/fc_tasks.c                    |  7 +++
 src/main/fc/settings.yaml                 |  2 +-
 src/main/scheduler/scheduler.h            |  1 +
 src/main/target/common_hardware.c         |  4 +-
 11 files changed, 134 insertions(+), 2 deletions(-)
 create mode 100644 src/main/drivers/accgyro/accgyro_bno055.c
 create mode 100644 src/main/drivers/accgyro/accgyro_bno055.h

diff --git a/make/source.mk b/make/source.mk
index 3a0d90d1e14..1da9d25bd44 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -66,6 +66,8 @@ COMMON_SRC = \
             drivers/temperature/ds18b20.c \
             drivers/temperature/lm75.c \
             drivers/pitotmeter_ms4525.c \
+            drivers/pitotmeter_ms4525.c \
+            drivers/accgyro/accgyro_bno055.c \
             fc/cli.c \
             fc/config.c \
             fc/controlrate_profile.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index b7453e88364..f4a03274d81 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -74,5 +74,6 @@ typedef enum {
     DEBUG_FFT,
     DEBUG_FFT_TIME,
     DEBUG_FFT_FREQ,
+    DEBUG_IMU2,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
new file mode 100644
index 00000000000..ef84a300901
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -0,0 +1,77 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "drivers/bus.h"
+#include "drivers/time.h"
+#include "build/debug.h"
+
+static busDevice_t * busDev;
+
+static bool deviceDetect(busDevice_t * busDev)
+{
+    for (int retry = 0; retry < 5; retry++) {
+        uint8_t sig;
+
+        delay(150);
+
+        bool ack = busRead(busDev, 0x00, &sig);
+        if (ack) {
+            return true;
+        }
+    };
+
+    return false;
+}
+
+bool bno055Init(void)
+{
+    busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
+    if (busDev == NULL) {
+        DEBUG_SET(DEBUG_IMU2, 2, 1);
+        return false;
+    }
+
+    if (!deviceDetect(busDev)) {
+        DEBUG_SET(DEBUG_IMU2, 2, 2);
+        busDeviceDeInit(busDev);
+        return false;
+    }
+
+    // /* Reset device */
+    // busWrite(busDev, PCA9685_MODE1, 0x00);
+
+    // /* Set refresh rate */
+    // pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY);
+
+    // delay(1);
+
+    // for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) {
+    //     pca9685setPWMOn(i, 0);
+    //     pca9685setPWMOff(i, 1500);
+    // }
+
+    return true;
+}
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
new file mode 100644
index 00000000000..d62af1d53ee
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -0,0 +1,25 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+bool bno055Init(void);
\ No newline at end of file
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 283d45e3f42..83a25a6c17c 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -142,6 +142,7 @@ typedef enum {
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_UG2864,       // I2C OLED display
     DEVHW_SDCARD,       // Generic SD-Card
+    DEVHW_BNO055,      // BNO055 IMU
 } devHardwareType_e;
 
 typedef enum {
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 497e153be6e..7066ed2e06d 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -37,6 +37,7 @@
 #include "drivers/time.h"
 #include "drivers/system.h"
 #include "drivers/pwm_output.h"
+#include "drivers/accgyro/accgyro_bno055.h"
 
 #include "sensors/sensors.h"
 #include "sensors/diagnostics.h"
@@ -748,6 +749,20 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens
     }
 }
 
+void taskSecondaryImu(timeUs_t currentTimeUs)
+{
+    static bool secondaryImuPresent = false;
+    static bool secondaryImuChecked = false;
+
+    if (!secondaryImuChecked) {
+        secondaryImuPresent = bno055Init();
+        secondaryImuChecked = true;
+    }
+
+    DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked);
+    DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent);
+}
+
 void taskMainPidLoop(timeUs_t currentTimeUs)
 {
     cycleTime = getTaskDeltaTime(TASK_SELF);
diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h
index a7317e48d1d..7caf10d0f59 100644
--- a/src/main/fc/fc_core.h
+++ b/src/main/fc/fc_core.h
@@ -46,3 +46,4 @@ bool isCalibrating(void);
 float getFlightTime(void);
 
 void fcReboot(bool bootLoader);
+void taskSecondaryImu(timeUs_t currentTimeUs);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 553c348ab68..26e5b0f9032 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -343,6 +343,7 @@ void fcTasksInit(void)
 #ifdef USE_GLOBAL_FUNCTIONS
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
+    setTaskEnabled(TASK_SECONDARY_IMU, true);
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -564,4 +565,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+    [TASK_SECONDARY_IMU] = {
+        .taskName = "IMU2",
+        .taskFunc = taskSecondaryImu,
+        .desiredPeriod = TASK_PERIOD_HZ(10),          // 10Hz @100msec
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
 };
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 65e47fd1d06..fdd241c4649 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -78,7 +78,7 @@ tables:
     values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", 
-      "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ"]
+      "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "DEBUG_IMU2"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 5cf996f02e4..4b1d7c252af 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -116,6 +116,7 @@ typedef enum {
 #ifdef USE_GLOBAL_FUNCTIONS
     TASK_GLOBAL_FUNCTIONS,
 #endif
+    TASK_SECONDARY_IMU,
     /* Count of real tasks */
     TASK_COUNT,
 
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 7c399dbd323..97eec0be1ca 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -312,7 +312,9 @@
             #define PCA9685_I2C_BUS BUS_I2C1
         #endif
         BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE);
-    #endif
+    #endif`
 #endif
 
+    BUSDEV_REGISTER_I2C(busdev_bno055,      DEVHW_BNO055,       BUS_I2C1,     0x29,               NONE,           DEVFLAGS_NONE);
+
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS

From d2bc52d1848185c51d0303b3de30ac0459f1605b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 28 Sep 2019 21:45:16 +0200
Subject: [PATCH 02/21] Read Euler roll

---
 src/main/drivers/accgyro/accgyro_bno055.c | 27 ++++++++++++++---------
 src/main/drivers/accgyro/accgyro_bno055.h | 12 +++++++++-
 src/main/fc/fc_core.c                     |  7 ++++++
 3 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index ef84a300901..f13f0d66bdf 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -27,6 +27,8 @@
 #include "drivers/bus.h"
 #include "drivers/time.h"
 #include "build/debug.h"
+#include "common/vector.h"
+#include "drivers/accgyro/accgyro_bno055.h"
 
 static busDevice_t * busDev;
 
@@ -60,18 +62,23 @@ bool bno055Init(void)
         return false;
     }
 
-    // /* Reset device */
-    // busWrite(busDev, PCA9685_MODE1, 0x00);
+    busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
+    delay(25);
+    busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF
+    delay(50);
 
-    // /* Set refresh rate */
-    // pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY);
+    return true;
+}
 
-    // delay(1);
+fpVector3_t bno055GetEurlerAngles(void)
+{
+    fpVector3_t eurlerAngles;
 
-    // for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) {
-    //     pca9685setPWMOn(i, 0);
-    //     pca9685setPWMOff(i, 1500);
-    // }
+    int8_t buf;
+    busRead(busDev, BNO055_ADDR_EUL_ROLL_LSB, &buf);
+    eurlerAngles.x = buf; 
+    busRead(busDev, BNO055_ADDR_EUL_ROLL_MSB, &buf);
+    eurlerAngles.x += buf << 8;
 
-    return true;
+    return eurlerAngles;
 }
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index d62af1d53ee..8493d7fcd4c 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -22,4 +22,14 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
-bool bno055Init(void);
\ No newline at end of file
+#define BNO055_ADDR_PWR_MODE 0x3E
+#define BNO055_ADDR_OPR_MODE 0x3D
+
+#define BNO055_PWR_MODE_NORMAL  0x00
+#define BNO055_OPR_MODE_NDOF    0x0C
+
+#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
+#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
+
+bool bno055Init(void);
+fpVector3_t bno055GetEurlerAngles(void)
\ No newline at end of file
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 7066ed2e06d..52eb1a653a0 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -761,6 +761,13 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
 
     DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked);
     DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent);
+
+    if (secondaryImuPresent) {
+        fpVector3_t eulerAngles = bno055GetEurlerAngles();
+        DEBUG_SET(DEBUG_IMU2, 5, eulerAngles.x);
+        DEBUG_SET(DEBUG_IMU2, 6, eulerAngles.y);
+        DEBUG_SET(DEBUG_IMU2, 7, eulerAngles.z);
+    }
 }
 
 void taskMainPidLoop(timeUs_t currentTimeUs)

From 853adf3872a762391988f9eaf08b3e20e85b8c5d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 28 Sep 2019 22:46:36 +0200
Subject: [PATCH 03/21] Read Euler angles from BNO055

---
 src/main/drivers/accgyro/accgyro_bno055.c | 11 ++++++-----
 src/main/drivers/accgyro/accgyro_bno055.h | 12 +++++++++---
 src/main/fc/fc_core.c                     |  1 +
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index f13f0d66bdf..3e6daab5118 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -74,11 +74,12 @@ fpVector3_t bno055GetEurlerAngles(void)
 {
     fpVector3_t eurlerAngles;
 
-    int8_t buf;
-    busRead(busDev, BNO055_ADDR_EUL_ROLL_LSB, &buf);
-    eurlerAngles.x = buf; 
-    busRead(busDev, BNO055_ADDR_EUL_ROLL_MSB, &buf);
-    eurlerAngles.x += buf << 8;
+    uint8_t buf[6];
+    busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
+
+    eurlerAngles.x = ((int16_t)((buf[3] << 8) | buf[2])) / 16;
+    eurlerAngles.y = ((int16_t)((buf[5] << 8) | buf[4])) / 16;
+    eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
 
     return eurlerAngles;
 }
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index 8493d7fcd4c..227346bd941 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -22,14 +22,20 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
+#include "common/vector.h"
+
 #define BNO055_ADDR_PWR_MODE 0x3E
 #define BNO055_ADDR_OPR_MODE 0x3D
 
 #define BNO055_PWR_MODE_NORMAL  0x00
 #define BNO055_OPR_MODE_NDOF    0x0C
 
-#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
-#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
+#define BNO055_ADDR_EUL_YAW_LSB     0x1A
+#define BNO055_ADDR_EUL_YAW_MSB     0x1B
+#define BNO055_ADDR_EUL_ROLL_LSB    0x1C
+#define BNO055_ADDR_EUL_ROLL_MSB    0x1D
+#define BNO055_ADDR_EUL_PITCH_LSB   0x1E
+#define BNO055_ADDR_EUL_PITCH_MSB   0x1F
 
 bool bno055Init(void);
-fpVector3_t bno055GetEurlerAngles(void)
\ No newline at end of file
+fpVector3_t bno055GetEurlerAngles(void);
\ No newline at end of file
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 52eb1a653a0..b8625d603ca 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -88,6 +88,7 @@
 #include "flight/failsafe.h"
 
 #include "config/feature.h"
+#include "common/vector.h"
 
 // June 2013     V2.2-dev
 

From 295be6996e67633db508821367268702ca2b8859 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 29 Sep 2019 10:53:15 +0200
Subject: [PATCH 04/21] Read calibration status

---
 src/main/drivers/accgyro/accgyro_bno055.c | 15 +++++++++++++++
 src/main/drivers/accgyro/accgyro_bno055.h | 15 ++++++++++++---
 src/main/fc/fc_core.c                     | 12 +++++++++---
 3 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 3e6daab5118..f06b6f66508 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -82,4 +82,19 @@ fpVector3_t bno055GetEurlerAngles(void)
     eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
 
     return eurlerAngles;
+}
+
+bno055CalibStat_t bno055GetCalibStat(void)
+{
+    bno055CalibStat_t stats;
+    uint8_t buf;
+
+    busRead(busDev, BNO055_ADDR_CALIB_STAT, &buf);
+
+    stats.mag = buf & 0b00000011;
+    stats.acc = (buf >> 2) & 0b00000011;
+    stats.gyr = (buf >> 4) & 0b00000011;
+    stats.sys = (buf >> 6) & 0b00000011;
+
+    return stats;
 }
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index 227346bd941..f36cb5f7aea 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -24,8 +24,9 @@
 
 #include "common/vector.h"
 
-#define BNO055_ADDR_PWR_MODE 0x3E
-#define BNO055_ADDR_OPR_MODE 0x3D
+#define BNO055_ADDR_PWR_MODE    0x3E
+#define BNO055_ADDR_OPR_MODE    0x3D
+#define BNO055_ADDR_CALIB_STAT  0x35
 
 #define BNO055_PWR_MODE_NORMAL  0x00
 #define BNO055_OPR_MODE_NDOF    0x0C
@@ -37,5 +38,13 @@
 #define BNO055_ADDR_EUL_PITCH_LSB   0x1E
 #define BNO055_ADDR_EUL_PITCH_MSB   0x1F
 
+typedef struct {
+    uint8_t sys;
+    uint8_t gyr;
+    uint8_t acc;
+    uint8_t mag;
+} bno055CalibStat_t;
+
 bool bno055Init(void);
-fpVector3_t bno055GetEurlerAngles(void);
\ No newline at end of file
+fpVector3_t bno055GetEurlerAngles(void);
+bno055CalibStat_t bno055GetCalibStat(void);
\ No newline at end of file
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index b8625d603ca..73953e474f9 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -765,9 +765,15 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
 
     if (secondaryImuPresent) {
         fpVector3_t eulerAngles = bno055GetEurlerAngles();
-        DEBUG_SET(DEBUG_IMU2, 5, eulerAngles.x);
-        DEBUG_SET(DEBUG_IMU2, 6, eulerAngles.y);
-        DEBUG_SET(DEBUG_IMU2, 7, eulerAngles.z);
+        DEBUG_SET(DEBUG_IMU2, 0, eulerAngles.x);
+        DEBUG_SET(DEBUG_IMU2, 1, eulerAngles.y);
+        DEBUG_SET(DEBUG_IMU2, 2, eulerAngles.z);
+
+        bno055CalibStat_t stats = bno055GetCalibStat();
+        DEBUG_SET(DEBUG_IMU2, 3, stats.mag);
+        DEBUG_SET(DEBUG_IMU2, 4, stats.acc);
+        DEBUG_SET(DEBUG_IMU2, 5, stats.gyr);
+        DEBUG_SET(DEBUG_IMU2, 6, stats.sys);
     }
 }
 

From 0c12340f6f24308611d81758fb86b92d596f9a02 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 29 Sep 2019 18:54:48 +0200
Subject: [PATCH 05/21] Rotate readouts to match body na groud frame

---
 make/source.mk                            |  1 +
 src/main/config/parameter_group_ids.h     |  3 +-
 src/main/drivers/accgyro/accgyro_bno055.c | 30 +++++++-
 src/main/drivers/accgyro/accgyro_bno055.h | 16 +----
 src/main/fc/fc_core.c                     | 27 --------
 src/main/fc/fc_core.h                     |  3 +-
 src/main/fc/fc_tasks.c                    |  7 +-
 src/main/fc/settings.yaml                 | 21 ++++++
 src/main/flight/secondary_imu.c           | 84 +++++++++++++++++++++++
 src/main/flight/secondary_imu.h           | 43 ++++++++++++
 src/main/scheduler/scheduler.h            |  2 +
 src/main/target/common.h                  |  3 +
 12 files changed, 194 insertions(+), 46 deletions(-)
 create mode 100644 src/main/flight/secondary_imu.c
 create mode 100644 src/main/flight/secondary_imu.h

diff --git a/make/source.mk b/make/source.mk
index 1da9d25bd44..303044c25e8 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -95,6 +95,7 @@ COMMON_SRC = \
             flight/servos.c \
             flight/wind_estimator.c \
             flight/gyroanalyse.c \
+            flight/secondary_imu.c \
             io/beeper.c \
             io/lights.c \
             io/pwmdriver_i2c.c \
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 53c31c31248..811b33028b4 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -108,7 +108,8 @@
 #define PG_RCDEVICE_CONFIG 1018
 #define PG_GENERAL_SETTINGS 1019
 #define PG_GLOBAL_FUNCTIONS 1020
-#define PG_INAV_END 1020
+#define PG_SECONDARY_IMU 1021
+#define PG_INAV_END 1021
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index f06b6f66508..97636e9c97f 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -30,6 +30,22 @@
 #include "common/vector.h"
 #include "drivers/accgyro/accgyro_bno055.h"
 
+#ifdef USE_IMU_BNO055
+
+#define BNO055_ADDR_PWR_MODE    0x3E
+#define BNO055_ADDR_OPR_MODE    0x3D
+#define BNO055_ADDR_CALIB_STAT  0x35
+
+#define BNO055_PWR_MODE_NORMAL  0x00
+#define BNO055_OPR_MODE_NDOF    0x0C
+
+#define BNO055_ADDR_EUL_YAW_LSB     0x1A
+#define BNO055_ADDR_EUL_YAW_MSB     0x1B
+#define BNO055_ADDR_EUL_ROLL_LSB    0x1C
+#define BNO055_ADDR_EUL_ROLL_MSB    0x1D
+#define BNO055_ADDR_EUL_PITCH_LSB   0x1E
+#define BNO055_ADDR_EUL_PITCH_MSB   0x1F
+
 static busDevice_t * busDev;
 
 static bool deviceDetect(busDevice_t * busDev)
@@ -70,6 +86,16 @@ bool bno055Init(void)
     return true;
 }
 
+void bno055FetchEulerAngles(int32_t * buffer)
+{
+    uint8_t buf[6];
+    busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
+
+    buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 16;
+    buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -16; //Pitch has to be reversed to match INAV notation
+    buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
+}
+
 fpVector3_t bno055GetEurlerAngles(void)
 {
     fpVector3_t eurlerAngles;
@@ -97,4 +123,6 @@ bno055CalibStat_t bno055GetCalibStat(void)
     stats.sys = (buf >> 6) & 0b00000011;
 
     return stats;
-}
\ No newline at end of file
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index f36cb5f7aea..c2f7ae5b793 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -21,23 +21,10 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
+#pragma once
 
 #include "common/vector.h"
 
-#define BNO055_ADDR_PWR_MODE    0x3E
-#define BNO055_ADDR_OPR_MODE    0x3D
-#define BNO055_ADDR_CALIB_STAT  0x35
-
-#define BNO055_PWR_MODE_NORMAL  0x00
-#define BNO055_OPR_MODE_NDOF    0x0C
-
-#define BNO055_ADDR_EUL_YAW_LSB     0x1A
-#define BNO055_ADDR_EUL_YAW_MSB     0x1B
-#define BNO055_ADDR_EUL_ROLL_LSB    0x1C
-#define BNO055_ADDR_EUL_ROLL_MSB    0x1D
-#define BNO055_ADDR_EUL_PITCH_LSB   0x1E
-#define BNO055_ADDR_EUL_PITCH_MSB   0x1F
-
 typedef struct {
     uint8_t sys;
     uint8_t gyr;
@@ -47,4 +34,5 @@ typedef struct {
 
 bool bno055Init(void);
 fpVector3_t bno055GetEurlerAngles(void);
+void bno055FetchEulerAngles(int32_t * buffer);
 bno055CalibStat_t bno055GetCalibStat(void);
\ No newline at end of file
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 73953e474f9..2cfd5a0ddf1 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -750,33 +750,6 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens
     }
 }
 
-void taskSecondaryImu(timeUs_t currentTimeUs)
-{
-    static bool secondaryImuPresent = false;
-    static bool secondaryImuChecked = false;
-
-    if (!secondaryImuChecked) {
-        secondaryImuPresent = bno055Init();
-        secondaryImuChecked = true;
-    }
-
-    DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked);
-    DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent);
-
-    if (secondaryImuPresent) {
-        fpVector3_t eulerAngles = bno055GetEurlerAngles();
-        DEBUG_SET(DEBUG_IMU2, 0, eulerAngles.x);
-        DEBUG_SET(DEBUG_IMU2, 1, eulerAngles.y);
-        DEBUG_SET(DEBUG_IMU2, 2, eulerAngles.z);
-
-        bno055CalibStat_t stats = bno055GetCalibStat();
-        DEBUG_SET(DEBUG_IMU2, 3, stats.mag);
-        DEBUG_SET(DEBUG_IMU2, 4, stats.acc);
-        DEBUG_SET(DEBUG_IMU2, 5, stats.gyr);
-        DEBUG_SET(DEBUG_IMU2, 6, stats.sys);
-    }
-}
-
 void taskMainPidLoop(timeUs_t currentTimeUs)
 {
     cycleTime = getTaskDeltaTime(TASK_SELF);
diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h
index 7caf10d0f59..34eb6fe68b6 100644
--- a/src/main/fc/fc_core.h
+++ b/src/main/fc/fc_core.h
@@ -45,5 +45,4 @@ void emergencyArmingUpdate(bool armingSwitchIsOn);
 bool isCalibrating(void);
 float getFlightTime(void);
 
-void fcReboot(bool bootLoader);
-void taskSecondaryImu(timeUs_t currentTimeUs);
+void fcReboot(bool bootLoader);
\ No newline at end of file
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 26e5b0f9032..e00dae77aff 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -47,6 +47,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/wind_estimator.h"
+#include "flight/secondary_imu.h"
 
 #include "navigation/navigation.h"
 
@@ -343,7 +344,9 @@ void fcTasksInit(void)
 #ifdef USE_GLOBAL_FUNCTIONS
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
-    setTaskEnabled(TASK_SECONDARY_IMU, true);
+#ifdef USE_SECONDARY_IMU
+    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -565,10 +568,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+#ifdef USE_SECONDARY_IMU
     [TASK_SECONDARY_IMU] = {
         .taskName = "IMU2",
         .taskFunc = taskSecondaryImu,
         .desiredPeriod = TASK_PERIOD_HZ(10),          // 10Hz @100msec
         .staticPriority = TASK_PRIORITY_IDLE,
     },
+#endif
 };
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index fdd241c4649..b4988d97ae8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -303,6 +303,27 @@ groups:
         type: uint8_t
         table: alignment
 
+  - name: PG_SECONDARY_IMU
+    type: secondaryImuConfig_t
+    headers: ["flight/secondary_imu.h"]
+    condition: USE_OPFLOW
+    members:
+      - name: imu2_enabled
+        field: enabled
+        type: bool
+      - name: imu2_align_roll
+        field: rollDeciDegrees
+        min: -1800
+        max: 3600
+      - name: imu2_align_pitch
+        field: pitchDeciDegrees
+        min: -1800
+        max: 3600
+      - name: imu2_align_yaw
+        field: yawDeciDegrees
+        min: -1800
+        max: 3600
+
   - name: PG_COMPASS_CONFIG
     type: compassConfig_t
     headers: ["sensors/compass.h"]
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
new file mode 100644
index 00000000000..34d494fe1b6
--- /dev/null
+++ b/src/main/flight/secondary_imu.c
@@ -0,0 +1,84 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "flight/secondary_imu.h"
+#include "config/parameter_group_ids.h"
+#include "sensors/boardalignment.h"
+
+#include "build/debug.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro_bno055.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 0);
+
+PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig,
+    .enabled = 0,
+    .rollDeciDegrees = 0,
+    .pitchDeciDegrees = 0,
+    .yawDeciDegrees = 0,
+);
+
+EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
+
+void taskSecondaryImu(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    static bool secondaryImuPresent = false;
+    static bool secondaryImuChecked = false;
+
+    if (!secondaryImuChecked) {
+        secondaryImuPresent = bno055Init();
+        secondaryImuChecked = true;
+    }
+
+    if (secondaryImuPresent) 
+    {
+        int32_t angles[3];
+        bno055FetchEulerAngles(angles);
+
+        const fpVector3_t v = {
+            .x = angles[0],
+            .y = angles[1],
+            .z = angles[2],
+         };
+
+        fpVector3_t rotated;
+
+        fp_angles_t compassAngles = {
+             .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
+             .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
+             .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
+        };
+        fpMat3_t rotationMatrix;
+        rotationMatrixFromAngles(&rotationMatrix, &compassAngles);
+        rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
+        rotated.z = ((int32_t)(rotated.z + DECIDEGREES_TO_DEGREES(secondaryImuConfig()->yawDeciDegrees))) % 360;
+
+        DEBUG_SET(DEBUG_IMU2, 0, rotated.x);
+        DEBUG_SET(DEBUG_IMU2, 1, rotated.y);
+        DEBUG_SET(DEBUG_IMU2, 2, rotated.z);
+        DEBUG_SET(DEBUG_IMU2, 3, angles[2]);
+    }
+}
\ No newline at end of file
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
new file mode 100644
index 00000000000..f69edb47d12
--- /dev/null
+++ b/src/main/flight/secondary_imu.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+#include "common/time.h"
+
+typedef struct secondaryImuConfig_s {
+    uint8_t enabled;
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+    int16_t yawDeciDegrees;
+} secondaryImuConfig_t;
+
+typedef struct secondaryImuState_s {
+    int16_t rawEulerAngles[3];
+} secondaryImuState_t;
+
+PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
+
+void taskSecondaryImu(timeUs_t currentTimeUs);
\ No newline at end of file
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 4b1d7c252af..b20f3a7066e 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -116,7 +116,9 @@ typedef enum {
 #ifdef USE_GLOBAL_FUNCTIONS
     TASK_GLOBAL_FUNCTIONS,
 #endif
+#ifdef USE_SECONDARY_IMU
     TASK_SECONDARY_IMU,
+#endif
     /* Count of real tasks */
     TASK_COUNT,
 
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 29d5598c20f..0d014216495 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -108,6 +108,9 @@
 #define USE_D_BOOST
 #define USE_ANTIGRAVITY
 
+#define USE_SECONDARY_IMU
+#define USE_IMU_BNO055
+
 #else // FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif

From 73b38494cebccd5bd3842f2f6ede12cab938b646 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 30 Sep 2019 17:12:10 +0200
Subject: [PATCH 06/21] Use secondary IMU for OSD heading and AHI

---
 src/main/drivers/accgyro/accgyro_bno055.c |  8 ++--
 src/main/drivers/accgyro/accgyro_bno055.h |  2 +-
 src/main/fc/settings.yaml                 |  6 +++
 src/main/flight/secondary_imu.c           | 50 +++++++++++++++--------
 src/main/flight/secondary_imu.h           | 10 ++++-
 src/main/io/osd.c                         | 35 ++++++++++++++--
 src/main/target/common.h                  |  3 --
 src/main/target/common_hardware.c         |  2 +-
 src/main/target/common_post.h             |  5 +++
 9 files changed, 92 insertions(+), 29 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 97636e9c97f..94aa3ef218a 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -86,14 +86,14 @@ bool bno055Init(void)
     return true;
 }
 
-void bno055FetchEulerAngles(int32_t * buffer)
+void bno055FetchEulerAngles(int16_t * buffer)
 {
     uint8_t buf[6];
     busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
 
-    buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 16;
-    buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -16; //Pitch has to be reversed to match INAV notation
-    buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
+    buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 1.6f;
+    buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
+    buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 1.6f;
 }
 
 fpVector3_t bno055GetEurlerAngles(void)
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index c2f7ae5b793..b6f7204cab9 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -34,5 +34,5 @@ typedef struct {
 
 bool bno055Init(void);
 fpVector3_t bno055GetEurlerAngles(void);
-void bno055FetchEulerAngles(int32_t * buffer);
+void bno055FetchEulerAngles(int16_t * buffer);
 bno055CalibStat_t bno055GetCalibStat(void);
\ No newline at end of file
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b4988d97ae8..f64d5ef8e5d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -311,6 +311,12 @@ groups:
       - name: imu2_enabled
         field: enabled
         type: bool
+      - name: imu2_use_for_osd_heading
+        field: useForOsdHeading
+        type: bool
+      - name: imu2_use_for_osd_ahi
+        field: useForOsdAHI
+        type: bool
       - name: imu2_align_roll
         field: rollDeciDegrees
         min: -1800
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 34d494fe1b6..e1f46d1fe0a 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -38,6 +38,8 @@ PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig,
     .rollDeciDegrees = 0,
     .pitchDeciDegrees = 0,
     .yawDeciDegrees = 0,
+    .useForOsdHeading = 0,
+    .useForOsdAHI = 0,
 );
 
 EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
@@ -45,40 +47,56 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
 void taskSecondaryImu(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
-    static bool secondaryImuPresent = false;
     static bool secondaryImuChecked = false;
 
     if (!secondaryImuChecked) {
-        secondaryImuPresent = bno055Init();
+        secondaryImuState.active = bno055Init();
         secondaryImuChecked = true;
     }
 
-    if (secondaryImuPresent) 
+    if (secondaryImuState.active) 
     {
-        int32_t angles[3];
-        bno055FetchEulerAngles(angles);
+        bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
 
+        //TODO this way of rotating a vector makes no sense, something simpler have to be developed
         const fpVector3_t v = {
-            .x = angles[0],
-            .y = angles[1],
-            .z = angles[2],
+            .x = secondaryImuState.eulerAngles.raw[0],
+            .y = secondaryImuState.eulerAngles.raw[1],
+            .z = secondaryImuState.eulerAngles.raw[2],
          };
 
         fpVector3_t rotated;
 
-        fp_angles_t compassAngles = {
+        fp_angles_t imuAngles = {
              .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
              .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
              .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
         };
         fpMat3_t rotationMatrix;
-        rotationMatrixFromAngles(&rotationMatrix, &compassAngles);
+        rotationMatrixFromAngles(&rotationMatrix, &imuAngles);
         rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
-        rotated.z = ((int32_t)(rotated.z + DECIDEGREES_TO_DEGREES(secondaryImuConfig()->yawDeciDegrees))) % 360;
+        rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600;
+
+        secondaryImuState.eulerAngles.values.roll = rotated.x;
+        secondaryImuState.eulerAngles.values.pitch = rotated.y;
+        secondaryImuState.eulerAngles.values.yaw = rotated.z;
+
+        static uint8_t tick = 0;
+        tick++;
+        if (tick == 10) {
+            secondaryImuState.calibrationStatus = bno055GetCalibStat();
+            tick = 0;
+        }
+
+        DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
+        DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
+        DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
+
+        DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
+        DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
+        DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
+        
 
-        DEBUG_SET(DEBUG_IMU2, 0, rotated.x);
-        DEBUG_SET(DEBUG_IMU2, 1, rotated.y);
-        DEBUG_SET(DEBUG_IMU2, 2, rotated.z);
-        DEBUG_SET(DEBUG_IMU2, 3, angles[2]);
     }
-}
\ No newline at end of file
+}
+
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index f69edb47d12..d86d9b566eb 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -26,18 +26,26 @@
 
 #include "config/parameter_group.h"
 #include "common/time.h"
+#include "sensors/sensors.h"
+#include "drivers/accgyro/accgyro_bno055.h"
 
 typedef struct secondaryImuConfig_s {
     uint8_t enabled;
     int16_t rollDeciDegrees;
     int16_t pitchDeciDegrees;
     int16_t yawDeciDegrees;
+    uint8_t useForOsdHeading;
+    uint8_t useForOsdAHI;
 } secondaryImuConfig_t;
 
 typedef struct secondaryImuState_s {
-    int16_t rawEulerAngles[3];
+    flightDynamicsTrims_t eulerAngles;
+    bno055CalibStat_t calibrationStatus;
+    uint8_t active;
 } secondaryImuState_t;
 
+extern secondaryImuState_t secondaryImuState;
+
 PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
 void taskSecondaryImu(timeUs_t currentTimeUs);
\ No newline at end of file
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index efca3f7f171..bb16a75d4df 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -81,6 +81,7 @@
 #include "flight/pid.h"
 #include "flight/rth_estimator.h"
 #include "flight/wind_estimator.h"
+#include "flight/secondary_imu.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -966,12 +967,28 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
 
 static bool osdIsHeadingValid(void)
 {
+#ifdef USE_SECONDARY_IMU
+    if (secondaryImuConfig()->useForOsdHeading) {
+        return true;
+    } else {
+        return isImuHeadingValid();
+    }
+#else 
     return isImuHeadingValid();
+#endif
 }
 
 int16_t osdGetHeading(void)
 {
-    return attitude.values.yaw;
+#ifdef USE_SECONDARY_IMU
+    if (secondaryImuConfig()->useForOsdHeading) {
+        return secondaryImuState.eulerAngles.values.yaw;
+    } else {
+        return attitude.values.yaw;
+    }
+#else 
+    return secondaryImuState.eulerAngles.values.yaw;
+#endif
 }
 
 // Returns a heading angle in degrees normalized to [0, 360).
@@ -1767,8 +1784,20 @@ static bool osdDrawSingleElement(uint8_t item)
 
             float pitch_rad_to_char = (float)(AH_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch);
 
-            float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
-            float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
+            float rollAngle;
+            float pitchAngle;
+#ifdef USE_SECONDARY_IMU
+            if (secondaryImuConfig()->useForOsdAHI) {
+                rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll);
+                pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
+            } else {
+                rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
+                pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);    
+            }
+#else
+            rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
+            pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
+#endif
 
             if (osdConfig()->ahi_reverse_roll) {
                 rollAngle = -rollAngle;
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 0d014216495..29d5598c20f 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -108,9 +108,6 @@
 #define USE_D_BOOST
 #define USE_ANTIGRAVITY
 
-#define USE_SECONDARY_IMU
-#define USE_IMU_BNO055
-
 #else // FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 97eec0be1ca..5e98cf3e4ee 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -312,7 +312,7 @@
             #define PCA9685_I2C_BUS BUS_I2C1
         #endif
         BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE);
-    #endif`
+    #endif
 #endif
 
     BUSDEV_REGISTER_I2C(busdev_bno055,      DEVHW_BNO055,       BUS_I2C1,     0x29,               NONE,           DEVFLAGS_NONE);
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index ca2da612e69..2c7cfe95061 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -52,6 +52,11 @@
 #undef USE_SERIALRX_JETIEXBUS
 #endif
 
+#if defined(USE_I2C) && (FLASH_SIZE > 256)
+#define USE_SECONDARY_IMU
+#define USE_IMU_BNO055
+#endif
+
 #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_DYNAMIC_FILTERS

From 51f6e0aac507618668fdfd2471461ac22d4bb2c1 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 30 Sep 2019 18:08:40 +0200
Subject: [PATCH 07/21] Fix compilation

---
 src/main/flight/secondary_imu.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index e1f46d1fe0a..3f6e734f95f 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -21,7 +21,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
-
+#include "common/utils.h"
 #include "flight/secondary_imu.h"
 #include "config/parameter_group_ids.h"
 #include "sensors/boardalignment.h"

From ac2f699bf3b5d44d282a100a7050ed9668e077fd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Oct 2019 10:33:47 +0200
Subject: [PATCH 08/21] Enable BNO055 only on selected boards

---
 src/main/target/MATEKF722/target.h | 4 ++++
 src/main/target/common_hardware.c  | 7 ++++++-
 src/main/target/common_post.h      | 5 -----
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h
index 7f43c4f1697..4d8d5cbb7bb 100755
--- a/src/main/target/MATEKF722/target.h
+++ b/src/main/target/MATEKF722/target.h
@@ -162,3 +162,7 @@
 #define MAX_PWM_OUTPUT_PORTS        7
 #define USE_DSHOT
 #define USE_SERIALSHOT
+
+#define USE_SECONDARY_IMU
+#define USE_IMU_BNO055
+#define BNO055_I2C_BUS          BUS_I2C1
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 5e98cf3e4ee..5ab3a8c1612 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -315,6 +315,11 @@
     #endif
 #endif
 
-    BUSDEV_REGISTER_I2C(busdev_bno055,      DEVHW_BNO055,       BUS_I2C1,     0x29,               NONE,           DEVFLAGS_NONE);
+#ifdef USE_IMU_BNO055
+#ifndef BNO055_I2C_BUS
+    #define BNO055_I2C_BUS BUS_I2C1
+#endif
+    BUSDEV_REGISTER_I2C(busdev_bno055,      DEVHW_BNO055,       BNO055_I2C_BUS,     0x29,               NONE,           DEVFLAGS_NONE);
+#endif
 
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 2c7cfe95061..ca2da612e69 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -52,11 +52,6 @@
 #undef USE_SERIALRX_JETIEXBUS
 #endif
 
-#if defined(USE_I2C) && (FLASH_SIZE > 256)
-#define USE_SECONDARY_IMU
-#define USE_IMU_BNO055
-#endif
-
 #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_DYNAMIC_FILTERS

From 9bb7768a66a73edfb78dc168fd48f653a41c79e1 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Dec 2019 14:28:05 +0100
Subject: [PATCH 09/21] Add interface to read and set BNO055 calibration

---
 src/main/drivers/accgyro/accgyro_bno055.c | 107 +++++++++++++++++-----
 src/main/drivers/accgyro/accgyro_bno055.h |  14 ++-
 src/main/flight/secondary_imu.c           |  27 +++---
 src/main/flight/secondary_imu.h           |   3 +
 4 files changed, 117 insertions(+), 34 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 94aa3ef218a..8a372c0c10f 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -32,31 +32,54 @@
 
 #ifdef USE_IMU_BNO055
 
-#define BNO055_ADDR_PWR_MODE    0x3E
-#define BNO055_ADDR_OPR_MODE    0x3D
-#define BNO055_ADDR_CALIB_STAT  0x35
-
-#define BNO055_PWR_MODE_NORMAL  0x00
-#define BNO055_OPR_MODE_NDOF    0x0C
-
-#define BNO055_ADDR_EUL_YAW_LSB     0x1A
-#define BNO055_ADDR_EUL_YAW_MSB     0x1B
-#define BNO055_ADDR_EUL_ROLL_LSB    0x1C
-#define BNO055_ADDR_EUL_ROLL_MSB    0x1D
-#define BNO055_ADDR_EUL_PITCH_LSB   0x1E
-#define BNO055_ADDR_EUL_PITCH_MSB   0x1F
-
-static busDevice_t * busDev;
-
-static bool deviceDetect(busDevice_t * busDev)
+#define BNO055_ADDR_PWR_MODE 0x3E
+#define BNO055_ADDR_OPR_MODE 0x3D
+#define BNO055_ADDR_CALIB_STAT 0x35
+
+#define BNO055_PWR_MODE_NORMAL 0x00
+#define BNO055_OPR_MODE_NDOF 0x0C
+
+#define BNO055_ADDR_EUL_YAW_LSB 0x1A
+#define BNO055_ADDR_EUL_YAW_MSB 0x1B
+#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
+#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
+#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
+#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
+
+#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
+#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
+#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
+#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
+#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
+#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
+
+#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
+#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
+#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
+#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
+#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
+#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
+
+#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
+#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
+#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
+#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
+#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
+#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
+
+static busDevice_t *busDev;
+
+static bool deviceDetect(busDevice_t *busDev)
 {
-    for (int retry = 0; retry < 5; retry++) {
+    for (int retry = 0; retry < 5; retry++)
+    {
         uint8_t sig;
 
         delay(150);
 
         bool ack = busRead(busDev, 0x00, &sig);
-        if (ack) {
+        if (ack)
+        {
             return true;
         }
     };
@@ -67,12 +90,14 @@ static bool deviceDetect(busDevice_t * busDev)
 bool bno055Init(void)
 {
     busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
-    if (busDev == NULL) {
+    if (busDev == NULL)
+    {
         DEBUG_SET(DEBUG_IMU2, 2, 1);
         return false;
     }
 
-    if (!deviceDetect(busDev)) {
+    if (!deviceDetect(busDev))
+    {
         DEBUG_SET(DEBUG_IMU2, 2, 2);
         busDeviceDeInit(busDev);
         return false;
@@ -86,7 +111,7 @@ bool bno055Init(void)
     return true;
 }
 
-void bno055FetchEulerAngles(int16_t * buffer)
+void bno055FetchEulerAngles(int16_t *buffer)
 {
     uint8_t buf[6];
     busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
@@ -125,4 +150,42 @@ bno055CalibStat_t bno055GetCalibStat(void)
     return stats;
 }
 
+bno055CalibrationData_t bno055GetCalibrationData(void)
+{
+    bno055CalibrationData_t data;
+    uint8_t buf[18];
+
+    busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18);
+
+    uint8_t bufferBit = 0;
+    for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            data.raw[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]);
+            bufferBit += 2;
+        }
+    }
+
+    return data;
+}
+
+void bno055SetCalibrationData(bno055CalibrationData_t data)
+{
+    uint8_t buf[18];
+
+    uint8_t bufferBit = 0;
+    for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            buf[bufferBit] = (uint8_t)(data.raw[sensorIndex][axisIndex] & 0xff);
+            buf[bufferBit + 1] = (uint8_t)((data.raw[sensorIndex][axisIndex] >> 8 ) & 0xff);
+            bufferBit += 2;
+        }
+    }
+
+    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18);
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index b6f7204cab9..e8c2eeb794b 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -32,7 +32,19 @@ typedef struct {
     uint8_t mag;
 } bno055CalibStat_t;
 
+typedef enum {
+    ACC = 0,
+    MAG = 1,
+    GYRO = 2
+} bno055Sensor_e;
+
+typedef struct {
+    int16_t raw[3][3];
+} bno055CalibrationData_t;
+
 bool bno055Init(void);
 fpVector3_t bno055GetEurlerAngles(void);
 void bno055FetchEulerAngles(int16_t * buffer);
-bno055CalibStat_t bno055GetCalibStat(void);
\ No newline at end of file
+bno055CalibStat_t bno055GetCalibStat(void);
+bno055CalibrationData_t bno055GetCalibrationData(void);
+void bno055SetCalibrationData(bno055CalibrationData_t data);
\ No newline at end of file
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 3f6e734f95f..e999c48735f 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -31,16 +31,23 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro_bno055.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 0);
+PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1);
 
-PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig,
-    .enabled = 0,
-    .rollDeciDegrees = 0,
-    .pitchDeciDegrees = 0,
-    .yawDeciDegrees = 0,
-    .useForOsdHeading = 0,
-    .useForOsdAHI = 0,
-);
+void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
+{
+    instance->enabled = 0;
+    instance->rollDeciDegrees = 0;
+    instance->pitchDeciDegrees = 0;
+    instance->yawDeciDegrees = 0;
+    instance->useForOsdHeading = 0;
+    instance->useForOsdAHI = 0;
+
+    for (uint8_t i = 0; i < 3 ; i++) {
+        instance->calibrationAcc[i] = 0;
+        instance->calibrationMag[i] = 0;
+        instance->calibrationGyro[i] = 0;
+    }
+}
 
 EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
 
@@ -95,8 +102,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
         DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
         DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
         DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
-        
-
     }
 }
 
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index d86d9b566eb..8209685c450 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -36,6 +36,9 @@ typedef struct secondaryImuConfig_s {
     int16_t yawDeciDegrees;
     uint8_t useForOsdHeading;
     uint8_t useForOsdAHI;
+    int16_t calibrationGyro[3];
+    int16_t calibrationMag[3];
+    int16_t calibrationAcc[3];
 } secondaryImuConfig_t;
 
 typedef struct secondaryImuState_s {

From 4b1acdd3eec8a28d0b6bf72511c987298d83adf0 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Dec 2019 14:59:16 +0100
Subject: [PATCH 10/21] Include radius in BNO055 calibration data

---
 src/main/drivers/accgyro/accgyro_bno055.c | 30 +++++++++++++++++------
 src/main/drivers/accgyro/accgyro_bno055.h |  3 ++-
 2 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 8a372c0c10f..92f9e31eb4d 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -46,6 +46,11 @@
 #define BNO055_ADDR_EUL_PITCH_LSB 0x1E
 #define BNO055_ADDR_EUL_PITCH_MSB 0x1F
 
+#define BNO055_ADDR_MAG_RADIUS_Z_MSB 0x6A 
+#define BNO055_ADDR_MAG_RADIUS_Z_LSB 0x69
+#define BNO055_ADDR_ACC_RADIUS_Z_MSB 0x68
+#define BNO055_ADDR_ACC_RADIUS_Z_LSB 0x67
+
 #define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
 #define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
 #define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
@@ -153,39 +158,50 @@ bno055CalibStat_t bno055GetCalibStat(void)
 bno055CalibrationData_t bno055GetCalibrationData(void)
 {
     bno055CalibrationData_t data;
-    uint8_t buf[18];
+    uint8_t buf[22];
 
-    busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18);
+    busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
 
     uint8_t bufferBit = 0;
     for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
     {
         for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
         {
-            data.raw[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]);
+            data.offset[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]);
             bufferBit += 2;
         }
     }
 
+    data.radius[ACC] = (int16_t)((buf[19] << 8) | buf[18]);
+    data.radius[MAG] = (int16_t)((buf[21] << 8) | buf[20]);
+
     return data;
 }
 
 void bno055SetCalibrationData(bno055CalibrationData_t data)
 {
-    uint8_t buf[18];
+    uint8_t buf[22];
 
+    //Prepare gains
     uint8_t bufferBit = 0;
     for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
     {
         for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
         {
-            buf[bufferBit] = (uint8_t)(data.raw[sensorIndex][axisIndex] & 0xff);
-            buf[bufferBit + 1] = (uint8_t)((data.raw[sensorIndex][axisIndex] >> 8 ) & 0xff);
+            buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
+            buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
             bufferBit += 2;
         }
     }
 
-    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18);
+    //Prepare radius
+    buf[18] = (uint8_t)(data.radius[ACC] & 0xff);
+    buf[19] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
+    buf[20] = (uint8_t)(data.radius[MAG] & 0xff);
+    buf[21] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
+
+    //Write to the device
+    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index e8c2eeb794b..df12fe5527d 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -39,7 +39,8 @@ typedef enum {
 } bno055Sensor_e;
 
 typedef struct {
-    int16_t raw[3][3];
+    int16_t radius[2];
+    int16_t offset[3][3];
 } bno055CalibrationData_t;
 
 bool bno055Init(void);

From ceaa0593e63d98458a091ca7163b06bf07e66c08 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Dec 2019 16:12:30 +0100
Subject: [PATCH 11/21] Restore calibration profile on device initalization

---
 src/main/drivers/accgyro/accgyro_bno055.c | 10 ++++++---
 src/main/drivers/accgyro/accgyro_bno055.h |  2 +-
 src/main/fc/settings.yaml                 |  4 ++++
 src/main/flight/secondary_imu.c           | 25 +++++++++++++++++++----
 src/main/flight/secondary_imu.h           |  8 +++++---
 5 files changed, 38 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 92f9e31eb4d..698195d0087 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -36,8 +36,9 @@
 #define BNO055_ADDR_OPR_MODE 0x3D
 #define BNO055_ADDR_CALIB_STAT 0x35
 
-#define BNO055_PWR_MODE_NORMAL 0x00
-#define BNO055_OPR_MODE_NDOF 0x0C
+#define BNO055_PWR_MODE_NORMAL  0x00
+#define BNO055_OPR_MODE_CONFIG  0x00
+#define BNO055_OPR_MODE_NDOF    0x0C
 
 #define BNO055_ADDR_EUL_YAW_LSB 0x1A
 #define BNO055_ADDR_EUL_YAW_MSB 0x1B
@@ -92,7 +93,7 @@ static bool deviceDetect(busDevice_t *busDev)
     return false;
 }
 
-bool bno055Init(void)
+bool bno055Init(bno055CalibrationData_t calibrationData)
 {
     busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
     if (busDev == NULL)
@@ -110,6 +111,9 @@ bool bno055Init(void)
 
     busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
     delay(25);
+    busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); //Set operational mode CONFIG_MODE
+    delay(30);
+    bno055SetCalibrationData(calibrationData);
     busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF
     delay(50);
 
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index df12fe5527d..86b18beccc9 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -43,7 +43,7 @@ typedef struct {
     int16_t offset[3][3];
 } bno055CalibrationData_t;
 
-bool bno055Init(void);
+bool bno055Init(bno055CalibrationData_t calibrationData);
 fpVector3_t bno055GetEurlerAngles(void);
 void bno055FetchEulerAngles(int16_t * buffer);
 bno055CalibStat_t bno055GetCalibStat(void);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 20ab9cd40ed..2ccb5bff3f9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -328,6 +328,10 @@ groups:
         field: yawDeciDegrees
         min: -1800
         max: 3600
+      - name: imu2_gain_acc_x
+        field: calibrationOffsetAcc[X]
+        min: INT16_MIN
+        max: INT16_MAX
 
   - name: PG_COMPASS_CONFIG
     type: compassConfig_t
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index e999c48735f..a94279b36cd 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -22,6 +22,7 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 #include "common/utils.h"
+#include "common/axis.h"
 #include "flight/secondary_imu.h"
 #include "config/parameter_group_ids.h"
 #include "sensors/boardalignment.h"
@@ -43,10 +44,12 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
     instance->useForOsdAHI = 0;
 
     for (uint8_t i = 0; i < 3 ; i++) {
-        instance->calibrationAcc[i] = 0;
-        instance->calibrationMag[i] = 0;
-        instance->calibrationGyro[i] = 0;
+        instance->calibrationOffsetGyro[i] = 0;
+        instance->calibrationOffsetMag[i] = 0;
+        instance->calibrationOffsetAcc[i] = 0;
     }
+    instance->calibrationRadiusAcc = 0;
+    instance->calibrationRadiusMag = 0;
 }
 
 EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
@@ -57,7 +60,21 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     static bool secondaryImuChecked = false;
 
     if (!secondaryImuChecked) {
-        secondaryImuState.active = bno055Init();
+
+        bno055CalibrationData_t calibrationData;
+        calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
+        calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
+        calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
+        calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
+        calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
+        calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
+        calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
+        calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
+        calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
+        calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
+        calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
+
+        secondaryImuState.active = bno055Init(calibrationData);
         secondaryImuChecked = true;
     }
 
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index 8209685c450..f0737d821e9 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -36,9 +36,11 @@ typedef struct secondaryImuConfig_s {
     int16_t yawDeciDegrees;
     uint8_t useForOsdHeading;
     uint8_t useForOsdAHI;
-    int16_t calibrationGyro[3];
-    int16_t calibrationMag[3];
-    int16_t calibrationAcc[3];
+    int16_t calibrationOffsetGyro[3];
+    int16_t calibrationOffsetMag[3];
+    int16_t calibrationOffsetAcc[3];
+    int16_t calibrationRadiusAcc;
+    int16_t calibrationRadiusMag;
 } secondaryImuConfig_t;
 
 typedef struct secondaryImuState_s {

From 91da35f23480caeadd3642fd214ff19147f80e11 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Dec 2019 11:51:45 +0100
Subject: [PATCH 12/21] Apply manual mag declination on secondary imu data

---
 src/main/flight/secondary_imu.c | 12 ++++++++++++
 src/main/flight/secondary_imu.h |  1 +
 src/main/io/osd.c               |  2 +-
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index a94279b36cd..56e59d9191b 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -26,6 +26,7 @@
 #include "flight/secondary_imu.h"
 #include "config/parameter_group_ids.h"
 #include "sensors/boardalignment.h"
+#include "sensors/compass.h"
 
 #include "build/debug.h"
 
@@ -56,11 +57,19 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
 
 void taskSecondaryImu(timeUs_t currentTimeUs)
 {
+    /*
+     * Secondary IMU works in decidegrees
+     */
     UNUSED(currentTimeUs);
     static bool secondaryImuChecked = false;
 
     if (!secondaryImuChecked) {
 
+        // Create magnetic declination matrix
+        const int deg = compassConfig()->mag_declination / 100;
+        const int min = compassConfig()->mag_declination % 100;
+        secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f;
+
         bno055CalibrationData_t calibrationData;
         calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
         calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
@@ -82,6 +91,9 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     {
         bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
 
+        //Apply mag declination
+        secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
+
         //TODO this way of rotating a vector makes no sense, something simpler have to be developed
         const fpVector3_t v = {
             .x = secondaryImuState.eulerAngles.raw[0],
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index f0737d821e9..51e917cc2a4 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -47,6 +47,7 @@ typedef struct secondaryImuState_s {
     flightDynamicsTrims_t eulerAngles;
     bno055CalibStat_t calibrationStatus;
     uint8_t active;
+    float magDeclination;
 } secondaryImuState_t;
 
 extern secondaryImuState_t secondaryImuState;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index b33a5f4b6f2..877597201a4 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1014,7 +1014,7 @@ int16_t osdGetHeading(void)
         return attitude.values.yaw;
     }
 #else 
-    return secondaryImuState.eulerAngles.values.yaw;
+    return attitude.values.yaw;
 #endif
 }
 

From 252894b9a9565cf44a1b3e6055dbbf22b293af82 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Dec 2019 14:25:00 +0100
Subject: [PATCH 13/21] Extract BNO055 initialization to separate routine

---
 src/main/fc/fc_init.c           |   4 +
 src/main/fc/fc_tasks.c          |   2 +-
 src/main/flight/secondary_imu.c | 145 ++++++++++++++++----------------
 src/main/flight/secondary_imu.h |   1 +
 4 files changed, 80 insertions(+), 72 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index e1718b26d38..7185a7146a1 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -637,6 +637,10 @@ void init(void)
     // Latch active features AGAIN since some may be modified by init().
     latchActiveFeatures();
     motorControlEnable = true;
+
+#ifdef USE_SECONDARY_IMU
+    secondaryImuInit();
+#endif
     fcTasksInit();
 
 #ifdef USE_OSD
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index b5e4f923bd0..6ac52006004 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -343,7 +343,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
 #ifdef USE_SECONDARY_IMU
-    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled);
+    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled && secondaryImuState.active);
 #endif
 }
 
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 56e59d9191b..b3fab753381 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -35,6 +35,8 @@
 
 PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1);
 
+EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
+
 void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
 {
     instance->enabled = 0;
@@ -44,7 +46,8 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
     instance->useForOsdHeading = 0;
     instance->useForOsdAHI = 0;
 
-    for (uint8_t i = 0; i < 3 ; i++) {
+    for (uint8_t i = 0; i < 3; i++)
+    {
         instance->calibrationOffsetGyro[i] = 0;
         instance->calibrationOffsetMag[i] = 0;
         instance->calibrationOffsetAcc[i] = 0;
@@ -53,84 +56,84 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
     instance->calibrationRadiusMag = 0;
 }
 
-EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
+void secondaryImuInit(void)
+{
+    // Create magnetic declination matrix
+    const int deg = compassConfig()->mag_declination / 100;
+    const int min = compassConfig()->mag_declination % 100;
+    secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f;
+
+    bno055CalibrationData_t calibrationData;
+    calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
+    calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
+    calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
+    calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
+    calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
+    calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
+    calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
+    calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
+    calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
+    calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
+    calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
+
+    secondaryImuState.active = bno055Init(calibrationData);
+}
 
 void taskSecondaryImu(timeUs_t currentTimeUs)
 {
+    if (!secondaryImuState.active)
+    {
+        return;
+    }
     /*
      * Secondary IMU works in decidegrees
      */
     UNUSED(currentTimeUs);
-    static bool secondaryImuChecked = false;
-
-    if (!secondaryImuChecked) {
-
-        // Create magnetic declination matrix
-        const int deg = compassConfig()->mag_declination / 100;
-        const int min = compassConfig()->mag_declination % 100;
-        secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f;
-
-        bno055CalibrationData_t calibrationData;
-        calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
-        calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
-        calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
-        calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
-        calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
-        calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
-        calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
-        calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
-        calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
-        calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
-        calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
-
-        secondaryImuState.active = bno055Init(calibrationData);
-        secondaryImuChecked = true;
-    }
 
-    if (secondaryImuState.active) 
+    bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
+
+    //Apply mag declination
+    secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
+
+    //TODO this way of rotating a vector makes no sense, something simpler have to be developed
+    const fpVector3_t v = {
+        .x = secondaryImuState.eulerAngles.raw[0],
+        .y = secondaryImuState.eulerAngles.raw[1],
+        .z = secondaryImuState.eulerAngles.raw[2],
+    };
+
+    fpVector3_t rotated;
+
+    fp_angles_t imuAngles = {
+        .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
+        .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
+        .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
+    };
+    fpMat3_t rotationMatrix;
+    rotationMatrixFromAngles(&rotationMatrix, &imuAngles);
+    rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
+    rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600;
+
+    secondaryImuState.eulerAngles.values.roll = rotated.x;
+    secondaryImuState.eulerAngles.values.pitch = rotated.y;
+    secondaryImuState.eulerAngles.values.yaw = rotated.z;
+
+    /*
+     * Every 10 cycles fetch current calibration state
+     */
+    static uint8_t tick = 0;
+    tick++;
+    if (tick == 10)
     {
-        bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
-
-        //Apply mag declination
-        secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
-
-        //TODO this way of rotating a vector makes no sense, something simpler have to be developed
-        const fpVector3_t v = {
-            .x = secondaryImuState.eulerAngles.raw[0],
-            .y = secondaryImuState.eulerAngles.raw[1],
-            .z = secondaryImuState.eulerAngles.raw[2],
-         };
-
-        fpVector3_t rotated;
-
-        fp_angles_t imuAngles = {
-             .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
-             .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
-             .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
-        };
-        fpMat3_t rotationMatrix;
-        rotationMatrixFromAngles(&rotationMatrix, &imuAngles);
-        rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
-        rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600;
-
-        secondaryImuState.eulerAngles.values.roll = rotated.x;
-        secondaryImuState.eulerAngles.values.pitch = rotated.y;
-        secondaryImuState.eulerAngles.values.yaw = rotated.z;
-
-        static uint8_t tick = 0;
-        tick++;
-        if (tick == 10) {
-            secondaryImuState.calibrationStatus = bno055GetCalibStat();
-            tick = 0;
-        }
-
-        DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
-        DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
-        DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
-
-        DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
-        DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
-        DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
+        secondaryImuState.calibrationStatus = bno055GetCalibStat();
+        tick = 0;
     }
-}
 
+    DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
+    DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
+    DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
+
+    DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
+    DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
+    DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
+}
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index 51e917cc2a4..f1a8f8a76cd 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -54,4 +54,5 @@ extern secondaryImuState_t secondaryImuState;
 
 PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
+void secondaryImuInit(void);
 void taskSecondaryImu(timeUs_t currentTimeUs);
\ No newline at end of file

From 97262b5cbe46d8ab29020ca9283e7e2c66dc1b87 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Dec 2019 15:22:29 +0100
Subject: [PATCH 14/21] CLI interface for BNO055 calibration data

---
 src/main/fc/cli.c               | 52 +++++++++++++++++++++++++++++++++
 src/main/flight/secondary_imu.c | 19 ++++++++++++
 src/main/flight/secondary_imu.h |  3 +-
 3 files changed, 73 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index e5ccef4bfa8..c6393d4a284 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -82,6 +82,7 @@ extern uint8_t __config_end;
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/secondary_imu.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -2683,6 +2684,54 @@ static void cliBatch(char *cmdline)
 }
 #endif
 
+#ifdef USE_SECONDARY_IMU
+
+static void printImu2Status(void)
+{
+    cliPrintLinef("Secondary IMU active: %d", secondaryImuState.active);
+    cliPrintLine("Calibration status:");
+    cliPrintLinef("Sys: %d", secondaryImuState.calibrationStatus.sys);
+    cliPrintLinef("Gyro: %d", secondaryImuState.calibrationStatus.gyr);
+    cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc);
+    cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag);
+    cliPrintLine("Calibration gains:");
+    cliPrintLinef(
+        "Gyro: %d %d %d", 
+        secondaryImuConfig()->calibrationOffsetGyro[X], 
+        secondaryImuConfig()->calibrationOffsetGyro[Y], 
+        secondaryImuConfig()->calibrationOffsetGyro[Z]
+    );
+    cliPrintLinef(
+        "Acc: %d %d %d", 
+        secondaryImuConfig()->calibrationOffsetAcc[X], 
+        secondaryImuConfig()->calibrationOffsetAcc[Y], 
+        secondaryImuConfig()->calibrationOffsetAcc[Z]
+    );
+    cliPrintLinef(
+        "Mag: %d %d %d", 
+        secondaryImuConfig()->calibrationOffsetMag[X], 
+        secondaryImuConfig()->calibrationOffsetMag[Y], 
+        secondaryImuConfig()->calibrationOffsetMag[Z]
+    );
+    cliPrintLinef(
+        "Radius: %d %d", 
+        secondaryImuConfig()->calibrationRadiusAcc, 
+        secondaryImuConfig()->calibrationRadiusMag
+    );
+}
+
+static void cliImu2(char *cmdline)
+{
+    if (sl_strcasecmp(cmdline, "fetch") == 0) {
+        secondaryImuFetchCalibration();
+        printImu2Status();
+    } else {
+        printImu2Status();
+    }
+}
+
+#endif
+
 static void cliSave(char *cmdline)
 {
     UNUSED(cmdline);
@@ -3369,6 +3418,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
 #endif
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
+#ifdef USE_SECONDARY_IMU
+    CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2),
+#endif
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
     CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
 #ifdef USE_SERIAL_PASSTHROUGH
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index b3fab753381..51b295751d8 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -137,3 +137,22 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
     DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
 }
+
+void secondaryImuFetchCalibration(void) {
+    bno055CalibrationData_t calibrationData = bno055GetCalibrationData();
+
+    secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X];
+    secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y];
+    secondaryImuConfigMutable()->calibrationOffsetAcc[Z] = calibrationData.offset[ACC][Z];
+
+    secondaryImuConfigMutable()->calibrationOffsetMag[X] = calibrationData.offset[MAG][X];
+    secondaryImuConfigMutable()->calibrationOffsetMag[Y] = calibrationData.offset[MAG][Y];
+    secondaryImuConfigMutable()->calibrationOffsetMag[Z] = calibrationData.offset[MAG][Z];
+
+    secondaryImuConfigMutable()->calibrationOffsetGyro[X] = calibrationData.offset[GYRO][X];
+    secondaryImuConfigMutable()->calibrationOffsetGyro[Y] = calibrationData.offset[GYRO][Y];
+    secondaryImuConfigMutable()->calibrationOffsetGyro[Z] = calibrationData.offset[GYRO][Z];
+
+    secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC];
+    secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG];
+}
\ No newline at end of file
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index f1a8f8a76cd..39c945e59d9 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -55,4 +55,5 @@ extern secondaryImuState_t secondaryImuState;
 PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
 void secondaryImuInit(void);
-void taskSecondaryImu(timeUs_t currentTimeUs);
\ No newline at end of file
+void taskSecondaryImu(timeUs_t currentTimeUs);
+void secondaryImuFetchCalibration(void);
\ No newline at end of file

From efeea7a501cdc83bf1706ff09cec087b302ec752 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 12 Dec 2019 22:15:04 +0100
Subject: [PATCH 15/21] Correctly fetch and restore calibration data

---
 src/main/drivers/accgyro/accgyro_bno055.c | 49 ++++++++++++++---------
 src/main/drivers/accgyro/accgyro_bno055.h |  2 +-
 src/main/fc/cli.c                         |  1 +
 src/main/fc/settings.yaml                 | 28 +++++++++++++
 src/main/flight/secondary_imu.c           | 27 +++++++------
 5 files changed, 75 insertions(+), 32 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c
index 698195d0087..fdb73b7250f 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.c
+++ b/src/main/drivers/accgyro/accgyro_bno055.c
@@ -47,10 +47,10 @@
 #define BNO055_ADDR_EUL_PITCH_LSB 0x1E
 #define BNO055_ADDR_EUL_PITCH_MSB 0x1F
 
-#define BNO055_ADDR_MAG_RADIUS_Z_MSB 0x6A 
-#define BNO055_ADDR_MAG_RADIUS_Z_LSB 0x69
-#define BNO055_ADDR_ACC_RADIUS_Z_MSB 0x68
-#define BNO055_ADDR_ACC_RADIUS_Z_LSB 0x67
+#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A 
+#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
+#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
+#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
 
 #define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
 #define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
@@ -93,7 +93,13 @@ static bool deviceDetect(busDevice_t *busDev)
     return false;
 }
 
-bool bno055Init(bno055CalibrationData_t calibrationData)
+static void bno055SetMode(uint8_t mode) 
+{
+    busWrite(busDev, BNO055_ADDR_OPR_MODE, mode);
+    delay(25);
+}
+
+bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration)
 {
     busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
     if (busDev == NULL)
@@ -111,11 +117,11 @@ bool bno055Init(bno055CalibrationData_t calibrationData)
 
     busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
     delay(25);
-    busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); //Set operational mode CONFIG_MODE
-    delay(30);
-    bno055SetCalibrationData(calibrationData);
-    busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF
-    delay(50);
+    if (setCalibration) {
+        bno055SetMode(BNO055_OPR_MODE_CONFIG);
+        bno055SetCalibrationData(calibrationData);
+    }
+    bno055SetMode(BNO055_OPR_MODE_NDOF);
 
     return true;
 }
@@ -164,8 +170,12 @@ bno055CalibrationData_t bno055GetCalibrationData(void)
     bno055CalibrationData_t data;
     uint8_t buf[22];
 
+    bno055SetMode(BNO055_OPR_MODE_CONFIG);
+
     busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
 
+    bno055SetMode(BNO055_OPR_MODE_NDOF);
+
     uint8_t bufferBit = 0;
     for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
     {
@@ -182,13 +192,14 @@ bno055CalibrationData_t bno055GetCalibrationData(void)
     return data;
 }
 
-void bno055SetCalibrationData(bno055CalibrationData_t data)
+void bno055SetCalibrationData(bno055CalibrationData_t data) 
 {
-    uint8_t buf[22];
+    uint8_t buf[12];
 
     //Prepare gains
+    //We do not restore gyro offsets, they are quickly calibrated at startup
     uint8_t bufferBit = 0;
-    for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
+    for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
     {
         for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
         {
@@ -198,14 +209,16 @@ void bno055SetCalibrationData(bno055CalibrationData_t data)
         }
     }
 
+    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
+
     //Prepare radius
-    buf[18] = (uint8_t)(data.radius[ACC] & 0xff);
-    buf[19] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
-    buf[20] = (uint8_t)(data.radius[MAG] & 0xff);
-    buf[21] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
+    buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
+    buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
+    buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
+    buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
 
     //Write to the device
-    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
+    busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h
index 86b18beccc9..d0c24f6dc4d 100644
--- a/src/main/drivers/accgyro/accgyro_bno055.h
+++ b/src/main/drivers/accgyro/accgyro_bno055.h
@@ -43,7 +43,7 @@ typedef struct {
     int16_t offset[3][3];
 } bno055CalibrationData_t;
 
-bool bno055Init(bno055CalibrationData_t calibrationData);
+bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
 fpVector3_t bno055GetEurlerAngles(void);
 void bno055FetchEulerAngles(int16_t * buffer);
 bno055CalibStat_t bno055GetCalibStat(void);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index c6393d4a284..35857499a1b 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -2695,6 +2695,7 @@ static void printImu2Status(void)
     cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc);
     cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag);
     cliPrintLine("Calibration gains:");
+    
     cliPrintLinef(
         "Gyro: %d %d %d", 
         secondaryImuConfig()->calibrationOffsetGyro[X], 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 2ccb5bff3f9..59ce6987aae 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -332,6 +332,34 @@ groups:
         field: calibrationOffsetAcc[X]
         min: INT16_MIN
         max: INT16_MAX
+      - name: imu2_gain_acc_y
+        field: calibrationOffsetAcc[Y]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_gain_acc_z
+        field: calibrationOffsetAcc[Z]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_gain_mag_x
+        field: calibrationOffsetMag[X]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_gain_mag_y
+        field: calibrationOffsetMag[Y]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_gain_mag_z
+        field: calibrationOffsetMag[Z]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_radius_acc
+        field: calibrationRadiusAcc
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: imu2_radius_mag
+        field: calibrationRadiusMag
+        min: INT16_MIN
+        max: INT16_MAX
 
   - name: PG_COMPASS_CONFIG
     type: compassConfig_t
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 51b295751d8..434b5248f6e 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -21,6 +21,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
+#include "stdint.h"
 #include "common/utils.h"
 #include "common/axis.h"
 #include "flight/secondary_imu.h"
@@ -64,19 +65,19 @@ void secondaryImuInit(void)
     secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f;
 
     bno055CalibrationData_t calibrationData;
-    calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
-    calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
-    calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
-    calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
-    calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
-    calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
-    calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
-    calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
-    calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
-    calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
-    calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
-
-    secondaryImuState.active = bno055Init(calibrationData);
+        calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
+        calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
+        calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
+        calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
+        calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
+        calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
+        calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
+        calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
+        calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
+        calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
+        calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
+
+    secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
 }
 
 void taskSecondaryImu(timeUs_t currentTimeUs)

From 22f9c481e8d0fc5d675489177566deb47f771506 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 4 Jan 2020 16:09:43 +0100
Subject: [PATCH 16/21] Enable BNO055 on Kakute F7

---
 src/main/target/KAKUTEF7/target.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 01984ed350a..6a8dbb95c23 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -181,3 +181,7 @@
 #define TARGET_IO_PORTE 0xffff
 
 #define MAX_PWM_OUTPUT_PORTS       6
+
+#define USE_SECONDARY_IMU
+#define USE_IMU_BNO055
+#define BNO055_I2C_BUS          BUS_I2C1
\ No newline at end of file

From 487745c34bd42a9c3dee64090c997ca7ee408f2b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 4 Jan 2020 16:44:55 +0100
Subject: [PATCH 17/21] Use automatic mag declination for secondary IMU

---
 src/main/flight/secondary_imu.c                | 8 +++++++-
 src/main/flight/secondary_imu.h                | 3 ++-
 src/main/navigation/navigation_pos_estimator.c | 7 ++++++-
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 434b5248f6e..3879227abaf 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -62,7 +62,8 @@ void secondaryImuInit(void)
     // Create magnetic declination matrix
     const int deg = compassConfig()->mag_declination / 100;
     const int min = compassConfig()->mag_declination % 100;
-    secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f;
+
+    secondaryImuSetMagneticDeclination(deg + min / 60.0f);
 
     bno055CalibrationData_t calibrationData;
         calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
@@ -137,6 +138,7 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
     DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
     DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
+    DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination);
 }
 
 void secondaryImuFetchCalibration(void) {
@@ -156,4 +158,8 @@ void secondaryImuFetchCalibration(void) {
 
     secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC];
     secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG];
+}
+
+void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees
+    secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees
 }
\ No newline at end of file
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index 39c945e59d9..749b3b575f6 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -56,4 +56,5 @@ PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
 void secondaryImuInit(void);
 void taskSecondaryImu(timeUs_t currentTimeUs);
-void secondaryImuFetchCalibration(void);
\ No newline at end of file
+void secondaryImuFetchCalibration(void);
+void secondaryImuSetMagneticDeclination(float declination);
\ No newline at end of file
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index d190c0c041e..4f3db5ccd29 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -39,6 +39,7 @@
 #include "fc/config.h"
 
 #include "flight/imu.h"
+#include "flight/secondary_imu.h"
 
 #include "io/gps.h"
 
@@ -213,7 +214,11 @@ void onNewGPSData(void)
         if(STATE(GPS_FIX_HOME)){
             static bool magDeclinationSet = false;
             if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
-                imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH));
+                const float declination = geoCalculateMagDeclination(&newLLH); 
+                imuSetMagneticDeclination(declination);
+#ifdef USE_SECONDARY_IMU
+                secondaryImuSetMagneticDeclination(declination);
+#endif
                 magDeclinationSet = true;
             }
         }

From e5b025378273502a202a9815951c4b5d906d91f6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 4 Jan 2020 17:36:32 +0100
Subject: [PATCH 18/21] Allow BNO055 data to be used in ANGLE mode

---
 src/main/fc/settings.yaml       |  3 +++
 src/main/flight/pid.c           | 16 ++++++++++++++++
 src/main/flight/secondary_imu.c |  1 +
 src/main/flight/secondary_imu.h |  1 +
 4 files changed, 21 insertions(+)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3967ac13b5c..a68d89a1185 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -319,6 +319,9 @@ groups:
       - name: imu2_use_for_osd_ahi
         field: useForOsdAHI
         type: bool
+      - name: imu2_use_for_stabilized
+        field: useForStabilized
+        type: bool
       - name: imu2_align_roll
         field: rollDeciDegrees
         min: -1800
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index ce7e6579828..e5a7eea21a6 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -42,6 +42,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/rpm_filter.h"
+#include "flight/secondary_imu.h"
 
 #include "io/gps.h"
 
@@ -533,7 +534,22 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
     if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
         angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
 
+#ifdef USE_SECONDARY_IMU
+    float actual;
+    if (secondaryImuConfig()->useForStabilized) {
+        if (axis == FD_ROLL) {
+            actual = secondaryImuState.eulerAngles.values.roll;
+        } else {
+            actual = secondaryImuState.eulerAngles.values.pitch;
+        }
+    } else {
+        actual = attitude.raw[axis];
+    }
+
+    const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual);
+#else
     const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
+#endif
 
     float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
 
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index 3879227abaf..e9c8fdb0e82 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -46,6 +46,7 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
     instance->yawDeciDegrees = 0;
     instance->useForOsdHeading = 0;
     instance->useForOsdAHI = 0;
+    instance->useForStabilized = 0;
 
     for (uint8_t i = 0; i < 3; i++)
     {
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index 749b3b575f6..b3d8f1da294 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -36,6 +36,7 @@ typedef struct secondaryImuConfig_s {
     int16_t yawDeciDegrees;
     uint8_t useForOsdHeading;
     uint8_t useForOsdAHI;
+    uint8_t useForStabilized;
     int16_t calibrationOffsetGyro[3];
     int16_t calibrationOffsetMag[3];
     int16_t calibrationOffsetAcc[3];

From 8eef9a5731cfc77fb5e78db59df3ada37b8266a6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 3 Apr 2021 22:28:39 +0200
Subject: [PATCH 19/21] Define BNO055 I2C on all supported targets

---
 src/main/target/AIKONF4/target.h           | 1 +
 src/main/target/AIRBOTF4/target.h          | 2 +-
 src/main/target/AIRBOTF7/target.h          | 2 +-
 src/main/target/ALIENFLIGHTF4/target.h     | 1 +
 src/main/target/ALIENFLIGHTNGF7/target.h   | 1 +
 src/main/target/ANYFC/target.h             | 1 +
 src/main/target/ANYFCF7/target.h           | 1 +
 src/main/target/ANYFCM7/target.h           | 1 +
 src/main/target/ASGARD32F4/target.h        | 1 +
 src/main/target/ASGARD32F7/target.h        | 1 +
 src/main/target/BEEROTORF4/target.h        | 1 +
 src/main/target/BETAFLIGHTF4/target.h      | 1 +
 src/main/target/BLUEJAYF4/target.h         | 1 +
 src/main/target/CLRACINGF4AIR/target.h     | 1 +
 src/main/target/COLIBRI/target.h           | 1 +
 src/main/target/COLIBRI_RACE/target.h      | 3 ++-
 src/main/target/DALRCF405/target.h         | 1 +
 src/main/target/DALRCF722DUAL/target.h     | 2 +-
 src/main/target/F4BY/target.h              | 1 +
 src/main/target/FALCORE/target.h           | 3 ++-
 src/main/target/FF_F35_LIGHTNING/target.h  | 1 +
 src/main/target/FIREWORKSV2/target.h       | 2 ++
 src/main/target/FISHDRONEF4/target.h       | 2 ++
 src/main/target/FLYWOOF411/target.h        | 2 ++
 src/main/target/FLYWOOF7DUAL/target.h      | 2 ++
 src/main/target/FOXEERF405/target.h        | 2 ++
 src/main/target/FOXEERF722DUAL/target.h    | 2 ++
 src/main/target/FRSKYPILOT/target.h        | 2 +-
 src/main/target/FRSKY_ROVERF7/target.h     | 2 +-
 src/main/target/FURYF4OSD/target.h         | 1 +
 src/main/target/HGLRCF722/target.h         | 2 +-
 src/main/target/IFLIGHTF4_SUCCEXD/target.h | 1 +
 src/main/target/IFLIGHTF4_TWING/target.h   | 1 +
 src/main/target/IFLIGHTF7_TWING/target.h   | 1 +
 src/main/target/KAKUTEF4/target.h          | 1 +
 src/main/target/KAKUTEF7/target.h          | 2 --
 src/main/target/KAKUTEF7MINIV3/target.h    | 1 +
 src/main/target/MAMBAF405US/target.h       | 2 ++
 src/main/target/MAMBAF722/target.h         | 2 ++
 src/main/target/MATEKF405/target.h         | 2 ++
 src/main/target/MATEKF405CAN/target.h      | 1 +
 src/main/target/MATEKF405SE/target.h       | 3 +--
 src/main/target/MATEKF411/target.h         | 1 +
 src/main/target/MATEKF411SE/target.h       | 1 +
 src/main/target/MATEKF722/target.h         | 2 --
 src/main/target/MATEKF722PX/target.h       | 2 +-
 src/main/target/MATEKF722SE/target.h       | 2 --
 src/main/target/MATEKF765/target.h         | 2 +-
 src/main/target/MATEKH743/target.h         | 2 +-
 src/main/target/MOTOLAB/target.h           | 3 ++-
 src/main/target/OMNIBUSF4/target.h         | 1 +
 src/main/target/OMNIBUSF7/target.h         | 1 +
 src/main/target/OMNIBUSF7NXT/target.h      | 1 +
 src/main/target/REVO/target.h              | 1 +
 src/main/target/RUSH_BLADE_F7/target.h     | 2 +-
 src/main/target/SPARKY2/target.h           | 1 +
 src/main/target/SPEEDYBEEF4/target.h       | 2 ++
 src/main/target/SPEEDYBEEF7/target.h       | 2 +-
 src/main/target/SPRACINGF4EVO/target.h     | 1 +
 src/main/target/SPRACINGF7DUAL/target.h    | 2 ++
 src/main/target/YUPIF4/target.h            | 1 +
 src/main/target/YUPIF7/target.h            | 1 +
 src/main/target/common.h                   | 3 +++
 63 files changed, 76 insertions(+), 22 deletions(-)

diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h
index 6e77fde6ad0..d0e13b8a122 100644
--- a/src/main/target/AIKONF4/target.h
+++ b/src/main/target/AIKONF4/target.h
@@ -103,6 +103,7 @@
 #define TEMPERATURE_I2C_BUS     DEFAULT_I2C_BUS
 #define PITOT_I2C_BUS           DEFAULT_I2C_BUS
 #define PCA9685_I2C_BUS         DEFAULT_I2C_BUS
+#define BNO055_I2C_BUS          DEFAULT_I2C_BUS
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h
index edf6973b54b..596a0b54fd9 100644
--- a/src/main/target/AIRBOTF4/target.h
+++ b/src/main/target/AIRBOTF4/target.h
@@ -64,8 +64,8 @@
 
 #define USE_PITOT_ADC
 #define PITOT_I2C_BUS           BUS_I2C2
-
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define M25P16_CS_PIN           PB3
 #define M25P16_SPI_BUS          BUS_SPI3
diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h
index 8b4a2677107..4a554cdd565 100644
--- a/src/main/target/AIRBOTF7/target.h
+++ b/src/main/target/AIRBOTF7/target.h
@@ -79,7 +79,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h
index 0cae3b81cb1..9d7718d6405 100644
--- a/src/main/target/ALIENFLIGHTF4/target.h
+++ b/src/main/target/ALIENFLIGHTF4/target.h
@@ -64,6 +64,7 @@
 #define MAG_MPU9250_ALIGN       CW0_DEG
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h
index 45e4fe08d96..0a4522d312c 100644
--- a/src/main/target/ALIENFLIGHTNGF7/target.h
+++ b/src/main/target/ALIENFLIGHTNGF7/target.h
@@ -66,6 +66,7 @@
 #define AK8963_SPI_BUS          BUS_SPI3
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h
index cfd82fc9c7f..ad5fdf17a78 100644
--- a/src/main/target/ANYFC/target.h
+++ b/src/main/target/ANYFC/target.h
@@ -53,6 +53,7 @@
 
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h
index 16baaca9cc9..1e6d6c9f42b 100644
--- a/src/main/target/ANYFCF7/target.h
+++ b/src/main/target/ANYFCF7/target.h
@@ -49,6 +49,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 
diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h
index ca42a0ad2e9..07e09434a11 100644
--- a/src/main/target/ANYFCM7/target.h
+++ b/src/main/target/ANYFCM7/target.h
@@ -49,6 +49,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h
index 49c8dd10c39..d9018a5cd92 100644
--- a/src/main/target/ASGARD32F4/target.h
+++ b/src/main/target/ASGARD32F4/target.h
@@ -168,3 +168,4 @@
 #define PITOT_I2C_BUS           BUS_I2C2
 #define PCA9685_I2C_BUS         BUS_I2C2
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h
index f26650eeeb9..97e2ad210e4 100644
--- a/src/main/target/ASGARD32F7/target.h
+++ b/src/main/target/ASGARD32F7/target.h
@@ -172,3 +172,4 @@
 
 #define PCA9685_I2C_BUS         BUS_I2C2
 #define PITOT_I2C_BUS           BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h
index 7f7ed3dea6d..7963cae436e 100644
--- a/src/main/target/BEEROTORF4/target.h
+++ b/src/main/target/BEEROTORF4/target.h
@@ -56,6 +56,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_OSD
 #define USE_MAX7456
diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h
index cf4727c7b3a..0b33a587556 100755
--- a/src/main/target/BETAFLIGHTF4/target.h
+++ b/src/main/target/BETAFLIGHTF4/target.h
@@ -130,6 +130,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index 06cca4cf11c..321b5e280ac 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -53,6 +53,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h
index 2b8f3c01459..e5f5aa5a4f8 100644
--- a/src/main/target/CLRACINGF4AIR/target.h
+++ b/src/main/target/CLRACINGF4AIR/target.h
@@ -116,6 +116,7 @@
 #define I2C2_SDA                PB11
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 #endif
 
 #define USE_ADC
diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h
index d852c19a061..eff71edd241 100755
--- a/src/main/target/COLIBRI/target.h
+++ b/src/main/target/COLIBRI/target.h
@@ -56,6 +56,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C3
+#define BNO055_I2C_BUS          BUS_I2C3
 
 #ifdef QUANTON
 #define IMU_MPU6000_ALIGN       CW90_DEG
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 57e6a98eeec..8347b58cd34 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -124,4 +124,5 @@
 #define TARGET_IO_PORTD         (BIT(2))
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(4))
 
-#define PCA9685_I2C_BUS         BUS_I2C2
\ No newline at end of file
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
\ No newline at end of file
diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h
index b478f8e2382..0b37dc2c11d 100644
--- a/src/main/target/DALRCF405/target.h
+++ b/src/main/target/DALRCF405/target.h
@@ -96,6 +96,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_HCSR04_I2C
diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h
index 534042b8dad..ca29604c6c4 100644
--- a/src/main/target/DALRCF722DUAL/target.h
+++ b/src/main/target/DALRCF722DUAL/target.h
@@ -82,7 +82,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define USE_SPI
diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h
index 6c2ebc79255..6dbb58442fb 100644
--- a/src/main/target/F4BY/target.h
+++ b/src/main/target/F4BY/target.h
@@ -56,6 +56,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h
index d8480eda73c..cdb8ba4636a 100755
--- a/src/main/target/FALCORE/target.h
+++ b/src/main/target/FALCORE/target.h
@@ -135,4 +135,5 @@
 #define TARGET_IO_PORTD         0xFFFF
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(4))
 
-#define PCA9685_I2C_BUS         BUS_I2C2
\ No newline at end of file
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
\ No newline at end of file
diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h
index d2285e8a382..73e3e0fee97 100644
--- a/src/main/target/FF_F35_LIGHTNING/target.h
+++ b/src/main/target/FF_F35_LIGHTNING/target.h
@@ -127,6 +127,7 @@
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_GPS | FEATURE_TELEMETRY)
 
diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h
index c3818458fd5..a0ea4428da4 100644
--- a/src/main/target/FIREWORKSV2/target.h
+++ b/src/main/target/FIREWORKSV2/target.h
@@ -231,6 +231,8 @@
 
 #if defined(OMNIBUSF4V6)
 #define PCA9685_I2C_BUS         BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 #else
 #define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 #endif
diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h
index 91fd29491b2..40c0fd199e0 100644
--- a/src/main/target/FISHDRONEF4/target.h
+++ b/src/main/target/FISHDRONEF4/target.h
@@ -65,6 +65,8 @@
 // *************** Temperature sensor *****************
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 // *************** BARO *****************************
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h
index 675513def2f..34d110d7a52 100644
--- a/src/main/target/FLYWOOF411/target.h
+++ b/src/main/target/FLYWOOF411/target.h
@@ -76,6 +76,8 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_HMC5883
diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h
index 419d5ba9e48..10b8d285bb6 100644
--- a/src/main/target/FLYWOOF7DUAL/target.h
+++ b/src/main/target/FLYWOOF7DUAL/target.h
@@ -113,6 +113,8 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_HMC5883
diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h
index c4c38aa3c7e..2b8d4e2ac0e 100644
--- a/src/main/target/FOXEERF405/target.h
+++ b/src/main/target/FOXEERF405/target.h
@@ -117,6 +117,8 @@
 #define USE_MAG_MAG3110
 #define USE_MAG_LIS3MDL
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 /*** ADC ***/
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN               PC0
diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h
index e1ac91c2716..b339cef7a9f 100644
--- a/src/main/target/FOXEERF722DUAL/target.h
+++ b/src/main/target/FOXEERF722DUAL/target.h
@@ -125,6 +125,8 @@
 #define USE_MAG_MAG3110
 #define USE_MAG_LIS3MDL
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 /*** ADC ***/
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN               PC0
diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h
index a92057f4eea..87bd0b072b0 100644
--- a/src/main/target/FRSKYPILOT/target.h
+++ b/src/main/target/FRSKYPILOT/target.h
@@ -123,7 +123,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C3
-
+#define BNO055_I2C_BUS          BUS_I2C3
 #define PITOT_I2C_BUS           BUS_I2C3
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h
index 30955d0bfd7..6347cd9bd91 100755
--- a/src/main/target/FRSKY_ROVERF7/target.h
+++ b/src/main/target/FRSKY_ROVERF7/target.h
@@ -66,7 +66,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 // *************** SPI2 Flash ***********************
diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h
index c4459596105..d5ccf15a952 100644
--- a/src/main/target/FURYF4OSD/target.h
+++ b/src/main/target/FURYF4OSD/target.h
@@ -134,6 +134,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS         DEFAULT_I2C_BUS
+#define BNO055_I2C_BUS              DEFAULT_I2C_BUS
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h
index c745a853be5..440a657c1b2 100644
--- a/src/main/target/HGLRCF722/target.h
+++ b/src/main/target/HGLRCF722/target.h
@@ -80,7 +80,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h
index e47b9c76381..aee436548c5 100644
--- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h
+++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h
@@ -112,6 +112,7 @@
 
 #define PITOT_I2C_BUS           BUS_I2C1
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h
index cbb9d29428e..ab5d0bf5f3f 100644
--- a/src/main/target/IFLIGHTF4_TWING/target.h
+++ b/src/main/target/IFLIGHTF4_TWING/target.h
@@ -79,6 +79,7 @@
 #define RANGEFINDER_I2C_BUS     BUS_I2C1
 
 #define PCA9685_I2C_BUS         BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 // *************** OSD *****************************
 #define USE_SPI_DEVICE_2
diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h
index bac28ceea0f..2fb765bce7e 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.h
+++ b/src/main/target/IFLIGHTF7_TWING/target.h
@@ -80,6 +80,7 @@
 
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 // *************** FLASH **************************
 #define M25P16_CS_PIN           PB9
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index 80013075a37..134889a0f3c 100755
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -65,6 +65,7 @@
 #   define USE_MAG_LIS3MDL
 
 #   define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS             BUS_I2C1
 
 #   define USE_BARO
 #   define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 0889e6d94fb..da8f8c9d1a9 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -180,6 +180,4 @@
 
 #define MAX_PWM_OUTPUT_PORTS       6
 
-#define USE_SECONDARY_IMU
-#define USE_IMU_BNO055
 #define BNO055_I2C_BUS          BUS_I2C1
\ No newline at end of file
diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h
index a32f9655c92..7ecc7a3d1ee 100644
--- a/src/main/target/KAKUTEF7MINIV3/target.h
+++ b/src/main/target/KAKUTEF7MINIV3/target.h
@@ -167,6 +167,7 @@
 #define RANGEFINDER_I2C_BUS         BUS_I2C1
 #define TEMPERATURE_I2C_BUS         BUS_I2C1
 #define PITOT_I2C_BUS               BUS_I2C1
+#define BNO055_I2C_BUS              BUS_I2C1
 
 /*** Used pins ***/
 #define TARGET_IO_PORTA         0xffff
diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h
index f4312c06fc6..f1ca6b7b006 100644
--- a/src/main/target/MAMBAF405US/target.h
+++ b/src/main/target/MAMBAF405US/target.h
@@ -56,6 +56,8 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
+#define BNO055_I2C_BUS                  BUS_I2C2
+
 //*********** Magnetometer / Compass *************
 #define USE_MAG
 #define MAG_I2C_BUS                     DEFAULT_I2C_BUS
diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h
index aa324c685c4..20bb2e4873b 100644
--- a/src/main/target/MAMBAF722/target.h
+++ b/src/main/target/MAMBAF722/target.h
@@ -56,6 +56,8 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
+#define BNO055_I2C_BUS                  BUS_I2C2
+
 //*********** Magnetometer / Compass *************
 #define USE_MAG
 #define MAG_I2C_BUS                     DEFAULT_I2C_BUS
diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h
index 37e7f78096c..a70cc555836 100644
--- a/src/main/target/MATEKF405/target.h
+++ b/src/main/target/MATEKF405/target.h
@@ -162,6 +162,8 @@
 
 #define TEMPERATURE_I2C_BUS         DEFAULT_I2C_BUS
 
+#define BNO055_I2C_BUS              DEFAULT_I2C_BUS
+
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
 #define USE_RANGEFINDER_HCSR04_I2C
diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h
index 057bf922e4f..f2c0999ee80 100644
--- a/src/main/target/MATEKF405CAN/target.h
+++ b/src/main/target/MATEKF405CAN/target.h
@@ -75,6 +75,7 @@
 #define PITOT_I2C_BUS           BUS_I2C2
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
 #define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 
 // *************** SPI2 RM3100 **************************
diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h
index 81681b94f55..79fc3347514 100644
--- a/src/main/target/MATEKF405SE/target.h
+++ b/src/main/target/MATEKF405SE/target.h
@@ -73,10 +73,9 @@
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_HCSR04_I2C
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
-
 #define PITOT_I2C_BUS           BUS_I2C2
-
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 
 // *************** SPI2 OSD ***************************
diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h
index b12f1e596ab..ac6962a3504 100755
--- a/src/main/target/MATEKF411/target.h
+++ b/src/main/target/MATEKF411/target.h
@@ -121,6 +121,7 @@
 #define USE_BARO_DPS310
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h
index de42b9c1ebe..59d59e42de1 100755
--- a/src/main/target/MATEKF411SE/target.h
+++ b/src/main/target/MATEKF411SE/target.h
@@ -113,6 +113,7 @@
 
 #define PITOT_I2C_BUS           BUS_I2C1
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h
index 317074b45b3..331ad658211 100755
--- a/src/main/target/MATEKF722/target.h
+++ b/src/main/target/MATEKF722/target.h
@@ -160,6 +160,4 @@
 #define USE_ESC_SENSOR
 #define USE_SERIALSHOT
 
-#define USE_SECONDARY_IMU
-#define USE_IMU_BNO055
 #define BNO055_I2C_BUS          BUS_I2C1
diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h
index 24c471860ed..ca9dd843a6b 100755
--- a/src/main/target/MATEKF722PX/target.h
+++ b/src/main/target/MATEKF722PX/target.h
@@ -66,7 +66,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h
index fc4a9b10e46..ecff68fa667 100644
--- a/src/main/target/MATEKF722SE/target.h
+++ b/src/main/target/MATEKF722SE/target.h
@@ -195,6 +195,4 @@
 #define USE_SERIALSHOT
 #define USE_ESC_SENSOR
 
-#define USE_SECONDARY_IMU
-#define USE_IMU_BNO055
 #define BNO055_I2C_BUS          BUS_I2C1
\ No newline at end of file
diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h
index 48dde29e1e0..8f5b2560264 100644
--- a/src/main/target/MATEKF765/target.h
+++ b/src/main/target/MATEKF765/target.h
@@ -86,7 +86,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
-
+#define BNO055_I2C_BUS          BUS_I2C2
 #define PITOT_I2C_BUS           BUS_I2C2
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h
index b8ac4303ebd..cc3409c8f52 100644
--- a/src/main/target/MATEKH743/target.h
+++ b/src/main/target/MATEKH743/target.h
@@ -157,7 +157,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
-
+#define BNO055_I2C_BUS          BUS_I2C2
 #define PITOT_I2C_BUS           BUS_I2C2
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index 386eadf1200..81d44eb6a20 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -120,4 +120,5 @@
 // !!TODO - check the following line is correct
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(3)|BIT(4))
 
-#define PCA9685_I2C_BUS         BUS_I2C2
\ No newline at end of file
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
\ No newline at end of file
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 0c3666a9d30..2a4383ab4dc 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -100,6 +100,7 @@
 #define USE_MAG_AK8975
 
 #define TEMPERATURE_I2C_BUS     I2C_EXT_BUS
+#define BNO055_I2C_BUS          I2C_EXT_BUS
 
 #define USE_BARO
 
diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h
index d013a36a6c1..d82ba047b15 100644
--- a/src/main/target/OMNIBUSF7/target.h
+++ b/src/main/target/OMNIBUSF7/target.h
@@ -158,6 +158,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_HCSR04_I2C
diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h
index b14e0c219ea..241322b5d6d 100644
--- a/src/main/target/OMNIBUSF7NXT/target.h
+++ b/src/main/target/OMNIBUSF7NXT/target.h
@@ -59,6 +59,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define USE_BARO_LPS25H
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
index be2c511c197..360d097c699 100644
--- a/src/main/target/REVO/target.h
+++ b/src/main/target/REVO/target.h
@@ -56,6 +56,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h
index ca0175f10df..cdae1860b3e 100644
--- a/src/main/target/RUSH_BLADE_F7/target.h
+++ b/src/main/target/RUSH_BLADE_F7/target.h
@@ -66,7 +66,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
-
+#define BNO055_I2C_BUS          BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h
index f716369b745..c1d18603d57 100644
--- a/src/main/target/SPARKY2/target.h
+++ b/src/main/target/SPARKY2/target.h
@@ -53,6 +53,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h
index 1365b7aec53..c51b5588320 100644
--- a/src/main/target/SPEEDYBEEF4/target.h
+++ b/src/main/target/SPEEDYBEEF4/target.h
@@ -124,6 +124,8 @@
 #define SERIAL_PORT_COUNT       6
 #endif
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 /*** BARO & MAG ***/
 #define USE_BARO							
 #define BARO_I2C_BUS          	BUS_I2C1	
diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h
index a5c8b9150ee..71e855af0ac 100644
--- a/src/main/target/SPEEDYBEEF7/target.h
+++ b/src/main/target/SPEEDYBEEF7/target.h
@@ -120,7 +120,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
-
+#define BNO055_I2C_BUS          BUS_I2C2
 #define PITOT_I2C_BUS           BUS_I2C2
 
 #define USE_RANGEFINDER
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index c1da04f60bc..efe89add1e3 100755
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -59,6 +59,7 @@
 #define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_VCP
 
diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
index aead3cbe598..9bbc2a74550 100644
--- a/src/main/target/SPRACINGF7DUAL/target.h
+++ b/src/main/target/SPRACINGF7DUAL/target.h
@@ -62,6 +62,8 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
+#define BNO055_I2C_BUS          BUS_I2C1
+
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG
 #define USE_MAG_HMC5883
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index dc423616f28..7f05ef7ef4d 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -68,6 +68,7 @@
 #define USE_MAG_QMC5883
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h
index b7799bfb819..87f600f32df 100644
--- a/src/main/target/YUPIF7/target.h
+++ b/src/main/target/YUPIF7/target.h
@@ -50,6 +50,7 @@
 #define USE_MAG_QMC5883
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 9d0ddc33462..18780d4f077 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -148,6 +148,9 @@
 #define USE_SERIALRX_GHST
 #define USE_TELEMETRY_GHST
 
+#define USE_SECONDARY_IMU
+#define USE_IMU_BNO055
+
 #else // MCU_FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif

From 44d2c08a819fed21381d7482a3664530ad81e667 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 4 Apr 2021 15:24:35 +0200
Subject: [PATCH 20/21] make BNO055 one of avalable secondary imus

---
 docs/Settings.md                | 30 ++++++++++++-------------
 src/main/fc/fc_tasks.c          |  2 +-
 src/main/fc/settings.yaml       | 39 ++++++++++++++++++++++++++++++---
 src/main/flight/pid.c           |  2 +-
 src/main/flight/secondary_imu.c | 12 ++++++++--
 src/main/flight/secondary_imu.h |  7 +++++-
 src/main/io/osd.c               |  6 ++---
 7 files changed, 72 insertions(+), 26 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 2f7054dc2ca..b775bb70cef 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -165,21 +165,21 @@
 | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) |
 | ibus_telemetry_type | 0 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. |
 | idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit |
-| imu2_align_pitch |  |  |
-| imu2_align_roll |  |  |
-| imu2_align_yaw |  |  |
-| imu2_enabled |  |  |
-| imu2_gain_acc_x |  |  |
-| imu2_gain_acc_y |  |  |
-| imu2_gain_acc_z |  |  |
-| imu2_gain_mag_x |  |  |
-| imu2_gain_mag_y |  |  |
-| imu2_gain_mag_z |  |  |
-| imu2_radius_acc |  |  |
-| imu2_radius_mag |  |  |
-| imu2_use_for_osd_ahi |  |  |
-| imu2_use_for_osd_heading |  |  |
-| imu2_use_for_stabilized |  |  |
+| imu2_align_pitch | 0 | Pitch alignment for Secondary IMU. 1/10 of a degree |
+| imu2_align_roll | 0 | Roll alignment for Secondary IMU. 1/10 of a degree |
+| imu2_align_yaw | 0 | Yaw alignment for Secondary IMU. 1/10 of a degree |
+| imu2_gain_acc_x | 0 | Secondary IMU ACC calibration data |
+| imu2_gain_acc_y | 0 | Secondary IMU ACC calibration data |
+| imu2_gain_acc_z | 0 | Secondary IMU ACC calibration data |
+| imu2_gain_mag_x | 0 | Secondary IMU MAG calibration data |
+| imu2_gain_mag_y | 0 | Secondary IMU MAG calibration data |
+| imu2_gain_mag_z | 0 | Secondary IMU MAG calibration data |
+| imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality |
+| imu2_radius_acc | 0 | Secondary IMU MAG calibration data |
+| imu2_radius_mag | 0 | Secondary IMU MAG calibration data |
+| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon |
+| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading |
+| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) |
 | imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes |
 | imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) |
 | imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements |
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 761483f7180..33d417a1d6f 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -377,7 +377,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
 #ifdef USE_SECONDARY_IMU
-    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled && secondaryImuState.active);
+    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active);
 #endif
 }
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 1dd75328be7..d95bb7e0ba5 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -9,6 +9,9 @@ tables:
   - name: rangefinder_hardware
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"]
     enum: rangefinderType_e
+  - name: secondary_imu_hardware
+    values: ["NONE", "BNO055"]
+    enum: secondaryImuType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"]
     enum: magSensor_e
@@ -409,60 +412,90 @@ groups:
     headers: ["flight/secondary_imu.h"]
     condition: USE_SECONDARY_IMU
     members:
-      - name: imu2_enabled
-        field: enabled
-        type: bool
+      - name: imu2_hardware
+        description: "Selection of a Secondary IMU hardware type. NONE disables this functionality"
+        default_value: "NONE"
+        field: hardwareType
+        table: secondary_imu_hardware
       - name: imu2_use_for_osd_heading
+        description: "If set to ON, Secondary IMU data will be used for Analog OSD heading"
+        default_value: "OFF"
         field: useForOsdHeading
         type: bool
       - name: imu2_use_for_osd_ahi
+        description: "If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon"
         field: useForOsdAHI
+        default_value: "OFF"
         type: bool
       - name: imu2_use_for_stabilized
+        description: "If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH)"
         field: useForStabilized
+        default_value: "OFF"
         type: bool
       - name: imu2_align_roll
+        description: "Roll alignment for Secondary IMU. 1/10 of a degree"
         field: rollDeciDegrees
+        default_value: "0"
         min: -1800
         max: 3600
       - name: imu2_align_pitch
+        description: "Pitch alignment for Secondary IMU. 1/10 of a degree"
         field: pitchDeciDegrees
+        default_value: "0"
         min: -1800
         max: 3600
       - name: imu2_align_yaw
+        description: "Yaw alignment for Secondary IMU. 1/10 of a degree"
         field: yawDeciDegrees
+        default_value: "0"
         min: -1800
         max: 3600
       - name: imu2_gain_acc_x
+        description: "Secondary IMU ACC calibration data"
         field: calibrationOffsetAcc[X]
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_gain_acc_y
         field: calibrationOffsetAcc[Y]
+        description: "Secondary IMU ACC calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_gain_acc_z
         field: calibrationOffsetAcc[Z]
+        description: "Secondary IMU ACC calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_gain_mag_x
         field: calibrationOffsetMag[X]
+        description: "Secondary IMU MAG calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_gain_mag_y
         field: calibrationOffsetMag[Y]
+        description: "Secondary IMU MAG calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_gain_mag_z
         field: calibrationOffsetMag[Z]
+        description: "Secondary IMU MAG calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_radius_acc
         field: calibrationRadiusAcc
+        description: "Secondary IMU MAG calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
       - name: imu2_radius_mag
         field: calibrationRadiusMag
+        description: "Secondary IMU MAG calibration data"
+        default_value: "0"
         min: INT16_MIN
         max: INT16_MAX
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index aa170855ae0..2bb7c4a204e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -555,7 +555,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 
 #ifdef USE_SECONDARY_IMU
     float actual;
-    if (secondaryImuConfig()->useForStabilized) {
+    if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) {
         if (axis == FD_ROLL) {
             actual = secondaryImuState.eulerAngles.values.roll;
         } else {
diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c
index e9c8fdb0e82..da3a4dfde72 100644
--- a/src/main/flight/secondary_imu.c
+++ b/src/main/flight/secondary_imu.c
@@ -40,7 +40,7 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
 
 void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
 {
-    instance->enabled = 0;
+    instance->hardwareType = SECONDARY_IMU_NONE,
     instance->rollDeciDegrees = 0;
     instance->pitchDeciDegrees = 0;
     instance->yawDeciDegrees = 0;
@@ -60,6 +60,7 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
 
 void secondaryImuInit(void)
 {
+    secondaryImuState.active = false;
     // Create magnetic declination matrix
     const int deg = compassConfig()->mag_declination / 100;
     const int min = compassConfig()->mag_declination % 100;
@@ -79,7 +80,14 @@ void secondaryImuInit(void)
         calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
         calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
 
-    secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+    if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
+        secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+    }
+
+    if (!secondaryImuState.active) {
+        secondaryImuConfigMutable()->hardwareType = SECONDARY_IMU_NONE;
+    }
+
 }
 
 void taskSecondaryImu(timeUs_t currentTimeUs)
diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h
index b3d8f1da294..b674bc9c931 100644
--- a/src/main/flight/secondary_imu.h
+++ b/src/main/flight/secondary_imu.h
@@ -29,8 +29,13 @@
 #include "sensors/sensors.h"
 #include "drivers/accgyro/accgyro_bno055.h"
 
+typedef enum {
+    SECONDARY_IMU_NONE      = 0,
+    SECONDARY_IMU_BNO055    = 1,
+} secondaryImuType_e;
+
 typedef struct secondaryImuConfig_s {
-    uint8_t enabled;
+    uint8_t hardwareType;
     int16_t rollDeciDegrees;
     int16_t pitchDeciDegrees;
     int16_t yawDeciDegrees;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 75d0bf1044a..ae636cba9ce 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -942,7 +942,7 @@ static inline int32_t osdGetAltitudeMsl(void)
 static bool osdIsHeadingValid(void)
 {
 #ifdef USE_SECONDARY_IMU
-    if (secondaryImuConfig()->useForOsdHeading) {
+    if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
         return true;
     } else {
         return isImuHeadingValid();
@@ -955,7 +955,7 @@ static bool osdIsHeadingValid(void)
 int16_t osdGetHeading(void)
 {
 #ifdef USE_SECONDARY_IMU
-    if (secondaryImuConfig()->useForOsdHeading) {
+    if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
         return secondaryImuState.eulerAngles.values.yaw;
     } else {
         return attitude.values.yaw;
@@ -1872,7 +1872,7 @@ static bool osdDrawSingleElement(uint8_t item)
             float pitchAngle;
 
 #ifdef USE_SECONDARY_IMU
-            if (secondaryImuConfig()->useForOsdAHI) {
+            if (secondaryImuState.active && secondaryImuConfig()->useForOsdAHI) {
                 rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll);
                 pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
             } else {

From f567a1b8e2b18f8195eb650f7b16bdfd2bf04146 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 4 Apr 2021 17:24:58 +0200
Subject: [PATCH 21/21] drop legacy file

---
 make/source.mk | 282 -------------------------------------------------
 1 file changed, 282 deletions(-)
 delete mode 100644 make/source.mk

diff --git a/make/source.mk b/make/source.mk
deleted file mode 100644
index e3a58d96dbb..00000000000
--- a/make/source.mk
+++ /dev/null
@@ -1,282 +0,0 @@
-COMMON_SRC = \
-            $(TARGET_DIR_SRC) \
-            main.c \
-            target/common_hardware.c \
-            build/assert.c \
-            build/build_config.c \
-            build/debug.c \
-            build/version.c \
-            common/bitarray.c \
-            common/calibration.c \
-            common/colorconversion.c \
-            common/crc.c \
-            common/encoding.c \
-            common/filter.c \
-            common/gps_conversion.c \
-            common/log.c \
-            common/logic_condition.c \
-            common/global_functions.c \
-            common/maths.c \
-            common/memory.c \
-            common/olc.c \
-            common/printf.c \
-            common/streambuf.c \
-            common/string_light.c \
-            common/time.c \
-            common/typeconversion.c \
-            common/uvarint.c \
-            config/config_eeprom.c \
-            config/config_streamer.c \
-            config/feature.c \
-            config/parameter_group.c \
-            config/general_settings.c \
-            drivers/adc.c \
-            drivers/buf_writer.c \
-            drivers/bus.c \
-            drivers/bus_busdev_i2c.c \
-            drivers/bus_busdev_spi.c \
-            drivers/bus_i2c_soft.c \
-            drivers/bus_spi.c \
-            drivers/display.c \
-            drivers/display_canvas.c \
-            drivers/display_font_metadata.c \
-            drivers/exti.c \
-            drivers/io.c \
-            drivers/io_pca9685.c \
-            drivers/light_led.c \
-            drivers/osd.c \
-            drivers/resource.c \
-            drivers/rx_nrf24l01.c \
-            drivers/rx_spi.c \
-            drivers/rx_xn297.c \
-            drivers/pitotmeter_adc.c \
-            drivers/pitotmeter_virtual.c \
-            drivers/pwm_esc_detect.c \
-            drivers/pwm_mapping.c \
-            drivers/pwm_output.c \
-            drivers/pinio.c \
-            drivers/rcc.c \
-            drivers/rx_pwm.c \
-            drivers/serial.c \
-            drivers/serial_uart.c \
-            drivers/sound_beeper.c \
-            drivers/stack_check.c \
-            drivers/system.c \
-            drivers/timer.c \
-            drivers/lights_io.c \
-            drivers/1-wire.c \
-            drivers/1-wire/ds_crc.c \
-            drivers/1-wire/ds2482.c \
-            drivers/temperature/ds18b20.c \
-            drivers/temperature/lm75.c \
-            drivers/pitotmeter_ms4525.c \
-            drivers/pitotmeter_ms4525.c \
-            drivers/accgyro/accgyro_bno055.c \
-            fc/cli.c \
-            fc/config.c \
-            fc/controlrate_profile.c \
-            fc/fc_core.c \
-            fc/fc_init.c \
-            fc/fc_tasks.c \
-            fc/fc_hardfaults.c \
-            fc/fc_msp.c \
-            fc/fc_msp_box.c \
-            fc/rc_smoothing.c \
-            fc/rc_adjustments.c \
-            fc/rc_controls.c \
-            fc/rc_curves.c \
-            fc/rc_modes.c \
-            fc/runtime_config.c \
-            fc/settings.c \
-            fc/stats.c \
-            flight/failsafe.c \
-            flight/hil.c \
-            flight/imu.c \
-            flight/mixer.c \
-            flight/pid.c \
-            flight/pid_autotune.c \
-            flight/rth_estimator.c \
-            flight/servos.c \
-            flight/wind_estimator.c \
-            flight/gyroanalyse.c \
-            flight/secondary_imu.c \
-            flight/rpm_filter.c \
-            flight/dynamic_gyro_notch.c \
-            io/beeper.c \
-            io/esc_serialshot.c \
-            io/frsky_osd.c \
-            io/osd_dji_hd.c \
-            io/lights.c \
-            io/piniobox.c \
-            io/pwmdriver_i2c.c \
-            io/serial.c \
-            io/serial_4way.c \
-            io/serial_4way_avrootloader.c \
-            io/serial_4way_stk500v2.c \
-            io/statusindicator.c \
-            io/rcdevice.c \
-            io/rcdevice_cam.c \
-            msp/msp_serial.c \
-            rx/crsf.c \
-            rx/eleres.c \
-            rx/fport.c \
-            rx/ibus.c \
-            rx/jetiexbus.c \
-            rx/msp.c \
-            rx/msp_override.c \
-            rx/nrf24_cx10.c \
-            rx/nrf24_inav.c \
-            rx/nrf24_h8_3d.c \
-            rx/nrf24_syma.c \
-            rx/nrf24_v202.c \
-            rx/pwm.c \
-            rx/rx.c \
-            rx/rx_spi.c \
-            rx/sbus.c \
-            rx/sbus_channels.c \
-            rx/spektrum.c \
-            rx/sumd.c \
-            rx/sumh.c \
-            rx/uib_rx.c \
-            rx/xbus.c \
-            scheduler/scheduler.c \
-            sensors/acceleration.c \
-            sensors/battery.c \
-            sensors/temperature.c \
-            sensors/boardalignment.c \
-            sensors/compass.c \
-            sensors/diagnostics.c \
-            sensors/gyro.c \
-            sensors/initialisation.c \
-            sensors/esc_sensor.c \
-            uav_interconnect/uav_interconnect_bus.c \
-            uav_interconnect/uav_interconnect_rangefinder.c \
-            blackbox/blackbox.c \
-            blackbox/blackbox_encoding.c \
-            blackbox/blackbox_io.c \
-            cms/cms.c \
-            cms/cms_menu_battery.c \
-            cms/cms_menu_blackbox.c \
-            cms/cms_menu_builtin.c \
-            cms/cms_menu_imu.c \
-            cms/cms_menu_ledstrip.c \
-            cms/cms_menu_misc.c \
-            cms/cms_menu_mixer_servo.c \
-            cms/cms_menu_navigation.c \
-            cms/cms_menu_osd.c \
-            cms/cms_menu_saveexit.c \
-            cms/cms_menu_vtx.c \
-            drivers/display_ug2864hsweg01.c \
-            drivers/rangefinder/rangefinder_hcsr04.c \
-            drivers/rangefinder/rangefinder_hcsr04_i2c.c \
-            drivers/rangefinder/rangefinder_srf10.c \
-            drivers/rangefinder/rangefinder_vl53l0x.c \
-            drivers/rangefinder/rangefinder_virtual.c \
-            drivers/opflow/opflow_fake.c \
-            drivers/opflow/opflow_virtual.c \
-            drivers/vtx_common.c \
-            io/rangefinder_msp.c \
-            io/rangefinder_benewake.c \
-            io/opflow_cxof.c \
-            io/opflow_msp.c \
-            io/dashboard.c \
-            io/displayport_frsky_osd.c \
-            io/displayport_max7456.c \
-            io/displayport_msp.c \
-            io/displayport_oled.c \
-            io/displayport_hott.c \
-            io/gps.c \
-            io/gps_ublox.c \
-            io/gps_nmea.c \
-            io/gps_naza.c \
-            io/ledstrip.c \
-            io/osd.c \
-            io/osd_canvas.c \
-            io/osd_common.c \
-            io/osd_grid.c \
-            io/osd_hud.c \
-            navigation/navigation.c \
-            navigation/navigation_fixedwing.c \
-            navigation/navigation_fw_launch.c \
-            navigation/navigation_geo.c \
-            navigation/navigation_multicopter.c \
-            navigation/navigation_pos_estimator.c \
-            navigation/navigation_pos_estimator_agl.c \
-            navigation/navigation_pos_estimator_flow.c \
-            sensors/barometer.c \
-            sensors/pitotmeter.c \
-            sensors/rangefinder.c \
-            sensors/opflow.c \
-            telemetry/crsf.c \
-            telemetry/frsky.c \
-            telemetry/frsky_d.c \
-            telemetry/hott.c \
-            telemetry/ibus_shared.c \
-            telemetry/ibus.c \
-            telemetry/ltm.c \
-            telemetry/mavlink.c \
-            telemetry/msp_shared.c \
-            telemetry/smartport.c \
-            telemetry/sim.c \
-            telemetry/telemetry.c \
-            io/vtx.c \
-            io/vtx_string.c \
-            io/vtx_smartaudio.c \
-            io/vtx_tramp.c \
-            io/vtx_ffpv24g.c \
-            io/vtx_control.c
-
-COMMON_DEVICE_SRC = \
-            $(CMSIS_SRC) \
-            $(DEVICE_STDPERIPH_SRC)
-
-TARGET_SRC := $(STARTUP_SRC) $(COMMON_DEVICE_SRC) $(COMMON_SRC) $(MCU_COMMON_SRC) $(TARGET_SRC)
-
-#excludes
-TARGET_SRC   := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC))
-
-ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
-TARGET_SRC += \
-            drivers/flash_m25p16.c \
-            io/flashfs.c \
-            $(MSC_SRC)
-endif
-
-ifneq ($(filter SDCARD,$(FEATURES)),)
-TARGET_SRC += \
-            drivers/sdcard/sdcard.c \
-            drivers/sdcard/sdcard_spi.c \
-            drivers/sdcard/sdcard_sdio.c \
-            drivers/sdcard/sdcard_standard.c \
-            io/asyncfatfs/asyncfatfs.c \
-            io/asyncfatfs/fat_standard.c \
-            $(MSC_SRC)
-endif
-
-ifneq ($(filter VCP,$(FEATURES)),)
-TARGET_SRC += $(VCP_SRC)
-endif
-
-ifneq ($(filter MSC,$(FEATURES)),)
-TARGET_SRC += $(MSC_SRC)
-endif
-
-ifneq ($(DSP_LIB),)
-
-INCLUDE_DIRS += $(DSP_LIB)/Include
-
-TARGET_SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
-
-TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
-TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
-
-TARGET_SRC += $(wildcard $(DSP_LIB)/Source/*/*.S)
-endif
-
-# Search path and source files for the ST stdperiph library
-VPATH        := $(VPATH):$(STDPERIPH_DIR)/src