Fetch Euler angles over BNO055 Serial protocol

src/main/drivers/accgyro/accgyro_bno055_serial.c

@@ -33,6 +33,7 @@
 #include "drivers/accgyro/accgyro_bno055.h"
 #include "build/debug.h"
 #include "drivers/time.h"
+#include "flight/secondary_imu.h"
 
 static serialPort_t * bno055SerialPort = NULL;
 
@@ -51,11 +52,17 @@ typedef enum {
     BNO055_FRAME_DATA,
 } bno055FrameType_e;
 
+typedef enum {
+    BNO055_DATA_TYPE_NONE,
+    BNO055_DATA_TYPE_EULER,
+} bno055DataType_e;
+
 static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE;
 static uint8_t bno055FrameType;
 static uint8_t bno055FrameLength;
 static uint8_t bno055FrameIndex;
 static timeMs_t bno055FrameStartAtMs = 0;
+static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE;
 
 static void bno055SerialWrite(uint8_t reg, uint8_t data) {
     bno055ProtocolState = BNO055_RECEIVE_IDLE;
@@ -66,12 +73,12 @@ static void bno055SerialWrite(uint8_t reg, uint8_t data) {
     serialWrite(bno055SerialPort, data);
 }
 
-static void bno055SerialRead(uint8_t reg) {
+static void bno055SerialRead(const uint8_t reg, const uint8_t len) {
     bno055ProtocolState = BNO055_RECEIVE_IDLE;
     serialWrite(bno055SerialPort, 0xAA); // Start Byte
     serialWrite(bno055SerialPort, 0x01); // Read command
     serialWrite(bno055SerialPort, reg);
-    serialWrite(bno055SerialPort, 1);
+    serialWrite(bno055SerialPort, len);
 }
 
 void bno055SerialDataReceive(uint16_t c, void *data) {
@@ -104,6 +111,15 @@ void bno055SerialDataReceive(uint16_t c, void *data) {
 
         if (bno055FrameIndex == bno055FrameLength) {
             bno055ProtocolState = BNO055_RECEIVE_IDLE;
+
+            if (bno055DataType == BNO055_DATA_TYPE_EULER) {
+                secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f;
+                secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
+                secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f;
+                secondaryImuProcess();
+            }
+
+            bno055DataType = BNO055_DATA_TYPE_NONE;
         }
     }
 
@@ -117,8 +133,6 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati
         return false;
     }
 
-    // DEBUG_SET(DEBUG_IMU2, 0, 2);
-
     bno055SerialPort = openSerialPort(
         portConfig->identifier,
         FUNCTION_BNO055,
@@ -133,44 +147,34 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati
         return false;
     }
 
-    bno055SerialRead(0x00); // Read ChipID
+    bno055SerialRead(0x00, 1); // Read ChipID
     delay(5);
 
-    DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType);
-    DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]);
-    DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState);
-    DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex);
-    
     //Check ident
     if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) {
         return false; // Ident does not match, leave
     }
 
-    // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
+    bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
-    // delay(5);
+    delay(25);
+
+    //TODO Here set calibration data 
+
+    bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set power mode NORMAL
+    delay(25);
 
 
-    // DEBUG_SET(DEBUG_IMU2, 0, 3);
+    // DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType);
-
+    // DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]);
-    // while (1) {
+    // DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState);
-    //     // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
+    // DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex);
-    //     // bno055SerialRead(0x00); //Set power mode NORMAL
-    //     bno055SerialRead(0x00); // Read ChipID
-    //     delay(100);
-    // }
-
-
-    // DEBUG_SET(DEBUG_IMU2, 0, 4);
 
     return true;
 }
 
-fpVector3_t bno055SerialGetEurlerAngles(void) {
+void bno055SerialFetchEulerAngles() {
-    
+    bno055DataType = BNO055_DATA_TYPE_EULER;
-}
+    bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6);
-
-void bno055SerialFetchEulerAngles(int16_t * buffer) {
-    // bno055SerialRead(0x00);
 }
 
 bno055CalibStat_t bno055SerialGetCalibStat(void) {

src/main/drivers/accgyro/accgyro_bno055_serial.h

@@ -28,8 +28,7 @@
 #include "drivers/accgyro/accgyro_bno055.h"
 
 bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration);
-fpVector3_t bno055SerialGetEurlerAngles(void);
+void bno055SerialFetchEulerAngles();
-void bno055SerialFetchEulerAngles(int16_t * buffer);
 bno055CalibStat_t bno055SerialGetCalibStat(void);
 bno055CalibrationData_t bno055SerialGetCalibrationData(void);
 void bno055SerialSetCalibrationData(bno055CalibrationData_t data);

src/main/fc/fc_tasks.c

@@ -605,7 +605,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SECONDARY_IMU] = {
         .taskName = "IMU2",
         .taskFunc = taskSecondaryImu,
-        .desiredPeriod = TASK_PERIOD_HZ(1),          // 10Hz @100msec
+        .desiredPeriod = TASK_PERIOD_HZ(10),          // 10Hz @100msec
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif

src/main/flight/secondary_imu.c

@@ -30,6 +30,7 @@
 #include "sensors/compass.h"
 
 #include "build/debug.h"
+#include "scheduler/scheduler.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro_bno055.h"
@@ -83,8 +84,17 @@ void secondaryImuInit(void)
 
     if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
         secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+
+        if (secondaryImuState.active) {
+            rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10));
+        }
+
     } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
         secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+
+        if (secondaryImuState.active) {
+            rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50));
+        }
     }
 
     if (!secondaryImuState.active) {
@@ -93,25 +103,7 @@ void secondaryImuInit(void)
 
 }
 
-void taskSecondaryImu(timeUs_t currentTimeUs)
+void secondaryImuProcess(void) {
-{
-    if (!secondaryImuState.active)
-    {
-        return;
-    }
-    /*
-     * Secondary IMU works in decidegrees
-     */
-    UNUSED(currentTimeUs);
-
-    if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
-        bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
-    } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
-        bno055SerialFetchEulerAngles(secondaryImuState.eulerAngles.raw);
-    }
-
-    bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
-
     //Apply mag declination
     secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
 
@@ -138,6 +130,31 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     secondaryImuState.eulerAngles.values.pitch = rotated.y;
     secondaryImuState.eulerAngles.values.yaw = rotated.z;
 
+    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 taskSecondaryImu(timeUs_t currentTimeUs)
+{
+    if (!secondaryImuState.active)
+    {
+        return;
+    }
+    /*
+     * Secondary IMU works in decidegrees
+     */
+    UNUSED(currentTimeUs);
+
+    if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
+        bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
+        secondaryImuProcess();
+
         /*
         * Every 10 cycles fetch current calibration state
         */
@@ -148,15 +165,9 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
             secondaryImuState.calibrationStatus = bno055GetCalibStat();
             tick = 0;
         }
-
+    } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
-    DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
+        bno055SerialFetchEulerAngles();
-    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) {

src/main/flight/secondary_imu.h

@@ -62,6 +62,7 @@ extern secondaryImuState_t secondaryImuState;
 
 PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
+void secondaryImuProcess(void);
 void secondaryImuInit(void);
 void taskSecondaryImu(timeUs_t currentTimeUs);
 void secondaryImuFetchCalibration(void);