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 {