Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Serial interface for BNO055 IMU #6802

Merged
merged 8 commits into from
Apr 16, 2021
Merged

Serial interface for BNO055 IMU #6802

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b1b8b99
Serial BNO055 detection
DzikuVx Apr 7, 2021
b529229
Fetch Euler angles over BNO055 Serial protocol
DzikuVx Apr 7, 2021
fcf07d1
Merge remote-tracking branch 'origin/master' into dzikuvx-bno055-serial
DzikuVx Apr 7, 2021
51f9c40
Fetch calibartion systus
DzikuVx Apr 8, 2021
d19837d
Read calibration data from BNO055 Serial
DzikuVx Apr 8, 2021
1e2df73
buffer write for BNO055 Serial interface
DzikuVx Apr 9, 2021
8cc66e5
Set calibration data
DzikuVx Apr 11, 2021
611e5a2
Rename serial port function
DzikuVx Apr 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/main/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,8 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_mpu9250.h
drivers/accgyro/accgyro_bno055.c
drivers/accgyro/accgyro_bno055.h
drivers/accgyro/accgyro_bno055_serial.c
drivers/accgyro/accgyro_bno055_serial.h

drivers/adc.c
drivers/adc.h
Expand Down
101 changes: 29 additions & 72 deletions src/main/drivers/accgyro/accgyro_bno055.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,47 +32,6 @@

#ifdef USE_IMU_BNO055

#define BNO055_ADDR_PWR_MODE 0x3E
#define BNO055_ADDR_OPR_MODE 0x3D
#define BNO055_ADDR_CALIB_STAT 0x35

#define BNO055_PWR_MODE_NORMAL 0x00
#define BNO055_OPR_MODE_CONFIG 0x00
#define BNO055_OPR_MODE_NDOF 0x0C

#define BNO055_ADDR_EUL_YAW_LSB 0x1A
#define BNO055_ADDR_EUL_YAW_MSB 0x1B
#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
#define BNO055_ADDR_EUL_PITCH_MSB 0x1F

#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A
#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
#define BNO055_ADDR_ACC_RADIUS_LSB 0x67

#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61

#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B

#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55

static busDevice_t *busDev;

static bool deviceDetect(busDevice_t *busDev)
Expand All @@ -99,18 +58,45 @@ static void bno055SetMode(uint8_t mode)
delay(25);
}

static void bno055SetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];

//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}

busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);

//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);

//Write to the device
busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
}

bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration)
{
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;
}
Expand Down Expand Up @@ -192,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void)
return data;
}

void bno055SetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];

//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}

busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);

//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);

//Write to the device
busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
}

#endif
44 changes: 42 additions & 2 deletions src/main/drivers/accgyro/accgyro_bno055.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,47 @@

#include "common/vector.h"

#define BNO055_ADDR_PWR_MODE 0x3E
#define BNO055_ADDR_OPR_MODE 0x3D
#define BNO055_ADDR_CALIB_STAT 0x35

#define BNO055_PWR_MODE_NORMAL 0x00
#define BNO055_OPR_MODE_CONFIG 0x00
#define BNO055_OPR_MODE_NDOF 0x0C

#define BNO055_ADDR_EUL_YAW_LSB 0x1A
#define BNO055_ADDR_EUL_YAW_MSB 0x1B
#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
#define BNO055_ADDR_EUL_PITCH_MSB 0x1F

#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A
#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
#define BNO055_ADDR_ACC_RADIUS_LSB 0x67

#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61

#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B

#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55

typedef struct {
uint8_t sys;
uint8_t gyr;
Expand All @@ -47,5 +88,4 @@ bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
fpVector3_t bno055GetEurlerAngles(void);
void bno055FetchEulerAngles(int16_t * buffer);
bno055CalibStat_t bno055GetCalibStat(void);
bno055CalibrationData_t bno055GetCalibrationData(void);
void bno055SetCalibrationData(bno055CalibrationData_t data);
bno055CalibrationData_t bno055GetCalibrationData(void);