From b529229a1ca00c01a50aa48962bc7129b81e5730 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 7 Apr 2021 20:10:30 +0200 Subject: [PATCH] Fetch Euler angles over BNO055 Serial protocol --- .../drivers/accgyro/accgyro_bno055_serial.c | 62 ++++++++-------- .../drivers/accgyro/accgyro_bno055_serial.h | 3 +- src/main/fc/fc_tasks.c | 2 +- src/main/flight/secondary_imu.c | 71 +++++++++++-------- src/main/flight/secondary_imu.h | 1 + 5 files changed, 77 insertions(+), 62 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index ca1ebfb50b..7955c9d7e8 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/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 - // delay(5); + bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + 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); - - // while (1) { - // // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL - // // bno055SerialRead(0x00); //Set power mode NORMAL - // bno055SerialRead(0x00); // Read ChipID - // delay(100); - // } - - - // DEBUG_SET(DEBUG_IMU2, 0, 4); + // 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); return true; } -fpVector3_t bno055SerialGetEurlerAngles(void) { - -} - -void bno055SerialFetchEulerAngles(int16_t * buffer) { - // bno055SerialRead(0x00); +void bno055SerialFetchEulerAngles() { + bno055DataType = BNO055_DATA_TYPE_EULER; + bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6); } bno055CalibStat_t bno055SerialGetCalibStat(void) { diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h index 5c0b4945e1..2db1e78f5f 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.h +++ b/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(int16_t * buffer); +void bno055SerialFetchEulerAngles(); bno055CalibStat_t bno055SerialGetCalibStat(void); bno055CalibrationData_t bno055SerialGetCalibrationData(void); void bno055SerialSetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index df5fc7357f..33d417a1d6 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index d47ee33712..ecdee8b33e 100644 --- a/src/main/flight/secondary_imu.c +++ b/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) -{ - 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); - +void secondaryImuProcess(void) { //Apply mag declination secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; @@ -138,17 +130,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs) 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); @@ -159,6 +140,36 @@ void taskSecondaryImu(timeUs_t currentTimeUs) // 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 + */ + static uint8_t tick = 0; + tick++; + if (tick == 10) + { + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; + } + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + bno055SerialFetchEulerAngles(); + } +} + void secondaryImuFetchCalibration(void) { bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 2c3b90ec6a..e7414a48c7 100644 --- a/src/main/flight/secondary_imu.h +++ b/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);