make BNO055 one of avalable secondary imus

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 |

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
 }
 

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
 

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 {

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)

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;

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 {