ALPAKA/driver-bmi088
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/driver-bmi088.git synced 2025-06-10 02:26:00 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added alternative conversion code

Browse Source
This commit is contained in:
dario 2024-07-17 09:13:48 +02:00
parent 1452c93161
commit 2b9dfda77c
2 changed files with 45 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/sta/drivers/bmi088.hpp
View File

@ -95,9 +95,11 @@ namespace sta
bool busRead(Part part, uint8_t reg, uint8_t * buffer); bool busRead(Part part, uint8_t reg, uint8_t * buffer);
bool busWrite(Part part, uint8_t reg, uint8_t value); bool busWrite(Part part, uint8_t reg, uint8_t value);
void getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z); void getRawRotation(int16_t* x, int16_t* y, int16_t* z);
void getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z); void getRawAcceleration(int16_t* x, int16_t* y, int16_t* z);
float lsbToMps2(int16_t val);
float lsbToDPS(int16_t val);
void convertRawToActual(uint16_t* i, float* f, float f_range); void convertRawToActual(uint16_t* i, float* f, float f_range);
Device* gyro_device; Device* gyro_device;

61
src/bmi088.cpp
View File

@ -1,6 +1,9 @@
#include <sta/drivers/bmi088.hpp> #include <sta/drivers/bmi088.hpp>
#include <sta/debug/assert.hpp> #include <sta/debug/assert.hpp>
#include <math.h>
#define GRAVITY 9.80665f
namespace sta namespace sta
{ {
@ -65,30 +68,31 @@ namespace sta
{ {
busWrite(GYROSCOPE, BMI088_REG_GYRO_BANDWIDTH, bandwidth); busWrite(GYROSCOPE, BMI088_REG_GYRO_BANDWIDTH, bandwidth);
} }
void BMI088::getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z) void BMI088::getRawRotation(int16_t* x, int16_t* y, int16_t* z)
{ {
uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb; uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb;
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_LSB, &x_lsb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_LSB, &x_lsb);
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_MSB, &x_msb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_MSB, &x_msb);
*x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb; *x = (int16_t)(((uint16_t)x_msb)<<8 | (uint16_t)x_lsb);
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_LSB, &y_lsb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_LSB, &y_lsb);
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_MSB, &y_msb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_MSB, &y_msb);
*y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb; *y = (int16_t)(((uint16_t)y_msb)<<8 | (uint16_t)y_lsb);
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_LSB, &z_lsb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_LSB, &z_lsb);
busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_MSB, &z_msb); busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_MSB, &z_msb);
*z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb; *z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);
} }
void BMI088::getRotation(float* x, float* y, float* z) void BMI088::getRotation(float* x, float* y, float* z)
{ {
uint16_t i_x,i_y,i_z; int16_t i_x,i_y,i_z;
getRawRotation(&i_x, &i_y, &i_z); getRawRotation(&i_x, &i_y, &i_z);
convertRawToActual(&i_x, x, f_gyro_range);
convertRawToActual(&i_y, y, f_gyro_range); *x = lsbToDPS(i_x);
convertRawToActual(&i_z, z, f_gyro_range); *y = lsbToDPS(i_y);
*z = lsbToDPS(i_z);
} }
@ -103,16 +107,16 @@ namespace sta
switch(range) switch(range)
{ {
case _3G: case _3G:
f_accel_range = 3*9.80665; f_accel_range = 3*GRAVITY;
break; break;
case _6G: case _6G:
f_accel_range = 6*9.80665; f_accel_range = 6*GRAVITY;
break; break;
case _12G: case _12G:
f_accel_range = 12*9.80665; f_accel_range = 12*GRAVITY;
break; break;
case _24G: case _24G:
f_accel_range = 24*9.80665; f_accel_range = 24*GRAVITY;
break; break;
default: default:
break; break;
@ -123,30 +127,31 @@ namespace sta
uint8_t data = bandwidth<<4 | odr;//TODO are your sure? shift three or four times? uint8_t data = bandwidth<<4 | odr;//TODO are your sure? shift three or four times?
busWrite(ACCELEROMETER, BMI088_REG_ACC_CONF, data); busWrite(ACCELEROMETER, BMI088_REG_ACC_CONF, data);
} }
void BMI088::getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z) void BMI088::getRawAcceleration(int16_t* x, int16_t* y, int16_t* z)
{ {
uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb; uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb;
busRead(ACCELEROMETER, BMI088_REG_ACC_X_LSB, &x_lsb); busRead(ACCELEROMETER, BMI088_REG_ACC_X_LSB, &x_lsb);
busRead(ACCELEROMETER, BMI088_REG_ACC_X_MSB, &x_msb); busRead(ACCELEROMETER, BMI088_REG_ACC_X_MSB, &x_msb);
*x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb; *x = (int16_t)(((uint16_t)x_msb)<<8 | (uint16_t)x_lsb);
busRead(ACCELEROMETER, BMI088_REG_ACC_Y_LSB, &y_lsb); busRead(ACCELEROMETER, BMI088_REG_ACC_Y_LSB, &y_lsb);
busRead(ACCELEROMETER, BMI088_REG_ACC_Y_MSB, &y_msb); busRead(ACCELEROMETER, BMI088_REG_ACC_Y_MSB, &y_msb);
*y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb; *y = (int16_t)(((uint16_t)y_msb)<<8 | (uint16_t)y_lsb);
busRead(ACCELEROMETER, BMI088_REG_ACC_Z_LSB, &z_lsb); busRead(ACCELEROMETER, BMI088_REG_ACC_Z_LSB, &z_lsb);
busRead(ACCELEROMETER, BMI088_REG_ACC_Z_MSB, &z_msb); busRead(ACCELEROMETER, BMI088_REG_ACC_Z_MSB, &z_msb);
*z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb; *z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);
} }
void BMI088::getAcceleration(float* x, float* y, float* z) void BMI088::getAcceleration(float* x, float* y, float* z)
{ {
uint16_t i_x,i_y,i_z; int16_t i_x,i_y,i_z;
getRawAcceleration(&i_x, &i_y, &i_z); getRawAcceleration(&i_x, &i_y, &i_z);
convertRawToActual(&i_x, x, f_accel_range);
convertRawToActual(&i_y, y, f_accel_range); *x = lsbToMps2(i_x);
convertRawToActual(&i_z, z, f_accel_range); *y = lsbToMps2(i_y);
*z = lsbToMps2(i_z);
} }
@ -190,6 +195,22 @@ namespace sta
return true; return true;
} }
float BMI088::lsbToMps2(int16_t val)
{
double power = 2;
float half_scale = (float)((pow((double)power, (double)16) / 2.0f));
return (val * f_accel_range) / half_scale;
}
float BMI088::lsbToDPS(int16_t val)
{
double power = 2;
float half_scale = (float)((pow((double)power, (double)16) / 2.0f));
return (f_gyro_range / (half_scale)) * (val);
}
void BMI088::convertRawToActual(uint16_t* i, float* f, float f_range){ void BMI088::convertRawToActual(uint16_t* i, float* f, float f_range){
int16_t sign = (int16_t)((*i & 0b1000000000000000) >> 15); int16_t sign = (int16_t)((*i & 0b1000000000000000) >> 15);
if(sign) if(sign)