Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

Fetch Euler angles over BNO055 Serial protocol

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-04-07 20:10:30 +02:00
parent b1b8b9973a
commit b529229a1c
5 changed files with 77 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

62
src/main/drivers/accgyro/accgyro_bno055_serial.c
View file

@ -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) {

3
src/main/drivers/accgyro/accgyro_bno055_serial.h
View file

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

2
src/main/fc/fc_tasks.c
View file

@ -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

71
src/main/flight/secondary_imu.c
View file

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

1
src/main/flight/secondary_imu.h
View file

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