Merge pull request #5100 from iNavFlight/dzikuvx-bno055-secondary-imu

BNO055 Secondary IMU

docs/Settings.md

@@ -165,6 +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 | 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 |

src/main/CMakeLists.txt

@@ -103,6 +103,8 @@ main_sources(COMMON_SRC
     drivers/accgyro/accgyro_mpu6500.h
     drivers/accgyro/accgyro_mpu9250.c
     drivers/accgyro/accgyro_mpu9250.h
+    drivers/accgyro/accgyro_bno055.c
+    drivers/accgyro/accgyro_bno055.h
 
     drivers/adc.c
     drivers/adc.h
@@ -333,6 +335,8 @@ main_sources(COMMON_SRC
     flight/dynamic_gyro_notch.h
     flight/dynamic_lpf.c
     flight/dynamic_lpf.h
+    flight/secondary_imu.c
+    flight/secondary_imu.h
 
     io/beeper.c
     io/beeper.h

src/main/build/debug.h

@@ -81,5 +81,6 @@ typedef enum {
     DEBUG_PCF8574,
     DEBUG_DYNAMIC_GYRO_LPF,
     DEBUG_FW_D,
+    DEBUG_IMU2,
     DEBUG_COUNT
 } debugType_e;

src/main/config/parameter_group_ids.h

@@ -116,7 +116,8 @@
 #define PG_SAFE_HOME_CONFIG 1026
 #define PG_DJI_OSD_CONFIG 1027
 #define PG_PROGRAMMING_PID 1028
-#define PG_INAV_END 1028
+#define PG_SECONDARY_IMU 1029
+#define PG_INAV_END 1029
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro_bno055.c

@@ -0,0 +1,224 @@
+/*
+ * 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"
+#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_CONFIG  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_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
+#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++)
+    {
+        uint8_t sig;
+
+        delay(150);
+
+        bool ack = busRead(busDev, 0x00, &sig);
+        if (ack)
+        {
+            return true;
+        }
+    };
+
+    return false;
+}
+
+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)
+    {
+        DEBUG_SET(DEBUG_IMU2, 2, 1);
+        return false;
+    }
+
+    if (!deviceDetect(busDev))
+    {
+        DEBUG_SET(DEBUG_IMU2, 2, 2);
+        busDeviceDeInit(busDev);
+        return false;
+    }
+
+    busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
+    delay(25);
+    if (setCalibration) {
+        bno055SetMode(BNO055_OPR_MODE_CONFIG);
+        bno055SetCalibrationData(calibrationData);
+    }
+    bno055SetMode(BNO055_OPR_MODE_NDOF);
+
+    return true;
+}
+
+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])) / 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)
+{
+    fpVector3_t eurlerAngles;
+
+    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;
+}
+
+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;
+}
+
+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++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            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[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 < 2; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            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, 12);
+
+    //Prepare radius
+    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_RADIUS_LSB, buf, 4);
+}
+
+#endif

src/main/drivers/accgyro/accgyro_bno055.h

@@ -0,0 +1,51 @@
+/*
+ * 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 "common/vector.h"
+
+typedef struct {
+    uint8_t sys;
+    uint8_t gyr;
+    uint8_t acc;
+    uint8_t mag;
+} bno055CalibStat_t;
+
+typedef enum {
+    ACC = 0,
+    MAG = 1,
+    GYRO = 2
+} bno055Sensor_e;
+
+typedef struct {
+    int16_t radius[2];
+    int16_t offset[3][3];
+} bno055CalibrationData_t;
+
+bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
+fpVector3_t bno055GetEurlerAngles(void);
+void bno055FetchEulerAngles(int16_t * buffer);
+bno055CalibStat_t bno055GetCalibStat(void);
+bno055CalibrationData_t bno055GetCalibrationData(void);
+void bno055SetCalibrationData(bno055CalibrationData_t data);

src/main/drivers/bus.h

@@ -150,6 +150,7 @@ typedef enum {
     DEVHW_SDCARD,       // Generic SD-Card
     DEVHW_IRLOCK,       // IR-Lock visual positioning hardware
     DEVHW_PCF8574,      // 8-bit I/O expander
+    DEVHW_BNO055,       // BNO055 IMU
 } devHardwareType_e;
 
 typedef enum {

src/main/fc/cli.c

@@ -85,6 +85,7 @@ uint8_t cliMode = 0;
 #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"
@@ -2918,6 +2919,55 @@ 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);
@@ -3666,6 +3716,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
 #if defined(USE_SAFE_HOME)
     CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes),
 #endif

src/main/fc/fc_core.c

@@ -38,6 +38,7 @@ FILE_COMPILE_FOR_SPEED
 #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"
@@ -89,6 +90,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/failsafe.h"
 
 #include "config/feature.h"
+#include "common/vector.h"
 #include "programming/pid.h"
 
 // June 2013     V2.2-dev

src/main/fc/fc_core.h

@@ -45,4 +45,4 @@ void emergencyArmingUpdate(bool armingSwitchIsOn);
 bool isCalibrating(void);
 float getFlightTime(void);
 
-void fcReboot(bool bootLoader);
+void fcReboot(bool bootLoader);

src/main/fc/fc_init.c

@@ -99,6 +99,7 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 #include "flight/rpm_filter.h"
+#include "flight/secondary_imu.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -680,6 +681,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

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 "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/dynamic_lpf.h"
@@ -375,6 +376,9 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+#ifdef USE_SECONDARY_IMU
+    setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -597,6 +601,14 @@ 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
 #ifdef USE_RPM_FILTER
     [TASK_RPM_FILTER] = {
         .taskName = "RPM",

src/main/fc/settings.yaml

@@ -9,6 +9,9 @@ tables:
   - name: rangefinder_hardware
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"]
     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
@@ -84,7 +87,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -404,6 +407,98 @@ groups:
         type: uint8_t
         table: alignment
 
+  - name: PG_SECONDARY_IMU
+    type: secondaryImuConfig_t
+    headers: ["flight/secondary_imu.h"]
+    condition: USE_SECONDARY_IMU
+    members:
+      - 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
+
   - name: PG_COMPASS_CONFIG
     type: compassConfig_t
     headers: ["sensors/compass.h"]

src/main/flight/pid.c

@@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/rpm_filter.h"
+#include "flight/secondary_imu.h"
 #include "flight/kalman.h"
 
 #include "io/gps.h"
@@ -538,8 +539,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && 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);
 
-
+        //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
-    //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
     if (axis == FD_PITCH && STATE(AIRPLANE)) {
         /* 
          * fixedWingLevelTrim has opposite sign to rcCommand.
@@ -553,7 +553,22 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
         angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);   
     }
 
+#ifdef USE_SECONDARY_IMU
+    float actual;
+    if (secondaryImuState.active && 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);
 

src/main/flight/secondary_imu.c

@@ -0,0 +1,174 @@
+/*
+ * 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 "stdint.h"
+#include "common/utils.h"
+#include "common/axis.h"
+#include "flight/secondary_imu.h"
+#include "config/parameter_group_ids.h"
+#include "sensors/boardalignment.h"
+#include "sensors/compass.h"
+
+#include "build/debug.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro_bno055.h"
+
+PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1);
+
+EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
+
+void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
+{
+    instance->hardwareType = SECONDARY_IMU_NONE,
+    instance->rollDeciDegrees = 0;
+    instance->pitchDeciDegrees = 0;
+    instance->yawDeciDegrees = 0;
+    instance->useForOsdHeading = 0;
+    instance->useForOsdAHI = 0;
+    instance->useForStabilized = 0;
+
+    for (uint8_t i = 0; i < 3; i++)
+    {
+        instance->calibrationOffsetGyro[i] = 0;
+        instance->calibrationOffsetMag[i] = 0;
+        instance->calibrationOffsetAcc[i] = 0;
+    }
+    instance->calibrationRadiusAcc = 0;
+    instance->calibrationRadiusMag = 0;
+}
+
+void secondaryImuInit(void)
+{
+    secondaryImuState.active = false;
+    // Create magnetic declination matrix
+    const int deg = compassConfig()->mag_declination / 100;
+    const int min = compassConfig()->mag_declination % 100;
+
+    secondaryImuSetMagneticDeclination(deg + min / 60.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;
+
+    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)
+{
+    if (!secondaryImuState.active)
+    {
+        return;
+    }
+    /*
+     * Secondary IMU works in decidegrees
+     */
+    UNUSED(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],
+        .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)
+    {
+        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, 6, secondaryImuState.magDeclination);
+}
+
+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];
+}
+
+void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees
+    secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees
+}

src/main/flight/secondary_imu.h

@@ -0,0 +1,66 @@
+/*
+ * 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"
+#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 hardwareType;
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+    int16_t yawDeciDegrees;
+    uint8_t useForOsdHeading;
+    uint8_t useForOsdAHI;
+    uint8_t useForStabilized;
+    int16_t calibrationOffsetGyro[3];
+    int16_t calibrationOffsetMag[3];
+    int16_t calibrationOffsetAcc[3];
+    int16_t calibrationRadiusAcc;
+    int16_t calibrationRadiusMag;
+} secondaryImuConfig_t;
+
+typedef struct secondaryImuState_s {
+    flightDynamicsTrims_t eulerAngles;
+    bno055CalibStat_t calibrationStatus;
+    uint8_t active;
+    float magDeclination;
+} secondaryImuState_t;
+
+extern secondaryImuState_t secondaryImuState;
+
+PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
+
+void secondaryImuInit(void);
+void taskSecondaryImu(timeUs_t currentTimeUs);
+void secondaryImuFetchCalibration(void);
+void secondaryImuSetMagneticDeclination(float declination);

src/main/io/osd.c

@@ -87,6 +87,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/pid.h"
 #include "flight/rth_estimator.h"
 #include "flight/wind_estimator.h"
+#include "flight/secondary_imu.h"
 #include "flight/servos.h"
 
 #include "navigation/navigation.h"
@@ -940,12 +941,28 @@ static inline int32_t osdGetAltitudeMsl(void)
 
 static bool osdIsHeadingValid(void)
 {
+#ifdef USE_SECONDARY_IMU
+    if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
+        return true;
+    } else {
+        return isImuHeadingValid();
+    }
+#else 
     return isImuHeadingValid();
+#endif
 }
 
 int16_t osdGetHeading(void)
 {
+#ifdef USE_SECONDARY_IMU
+    if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
+        return secondaryImuState.eulerAngles.values.yaw;
+    } else {
+        return attitude.values.yaw;
+    }
+#else 
     return attitude.values.yaw;
+#endif
 }
 
 int16_t osdPanServoHomeDirectionOffset(void)
@@ -1851,8 +1868,21 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_ARTIFICIAL_HORIZON:
         {
-            float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
+            float rollAngle;
-            float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
+            float pitchAngle;
+
+#ifdef USE_SECONDARY_IMU
+            if (secondaryImuState.active && 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;

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"
 
@@ -214,7 +215,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;
             }
         }

src/main/scheduler/scheduler.h

@@ -119,6 +119,9 @@ typedef enum {
 #endif
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
+#endif
+#ifdef USE_SECONDARY_IMU
+    TASK_SECONDARY_IMU,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

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

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

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

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

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

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

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
 

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

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

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

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

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

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

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

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

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
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2

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

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

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

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
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2

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)
 

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

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

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

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

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

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

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

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 ***********************

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

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

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

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

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

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

src/main/target/KAKUTEF7/target.h

@@ -179,3 +179,5 @@
 #define TARGET_IO_PORTE 0xffff
 
 #define MAX_PWM_OUTPUT_PORTS       6
+
+#define BNO055_I2C_BUS          BUS_I2C1

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

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

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

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

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 **************************

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 ***************************

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

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

src/main/target/MATEKF722/target.h

@@ -159,3 +159,5 @@
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 #define USE_SERIALSHOT
+
+#define BNO055_I2C_BUS          BUS_I2C1

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

src/main/target/MATEKF722SE/target.h

@@ -193,4 +193,6 @@
 #define MAX_PWM_OUTPUT_PORTS        8
 #define USE_DSHOT
 #define USE_SERIALSHOT
-#define USE_ESC_SENSOR
+#define USE_ESC_SENSOR
+
+#define BNO055_I2C_BUS          BUS_I2C1

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

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

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
+#define PCA9685_I2C_BUS         BUS_I2C2
+#define BNO055_I2C_BUS          BUS_I2C2

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
 

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

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

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

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

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

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	

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

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
 

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

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

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

src/main/target/common.h

@@ -149,6 +149,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

src/main/target/common_hardware.c

@@ -394,4 +394,11 @@
     BUSDEV_REGISTER_I2C(busdev_pcf8574,      DEVHW_PCF8574,       PCF8574_I2C_BUS,     0x20,               NONE,           DEVFLAGS_NONE, 0);
 #endif
 
+#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, 0);
+#endif
+
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS