dzukuvx-bno055-secondary-imu

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 \

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;

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;
+}

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);

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 {

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);

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);

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,
+    },
 };

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

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,
 

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