driver-bmi088/src/bmi088.cpp

#include <sta/drivers/bmi088.hpp>

#include <sta/debug/assert.hpp>
#include <math.h>

#define GRAVITY		9.80665f

#include <sta/config.hpp>
#ifndef STA_SPATZ_ENABLED

namespace sta
{
    BMI088::BMI088(SPIDevice* gyro_device, SPIDevice* accel_device)
        : gyro_device{gyro_device}, accel_device(accel_device)
    {
        STA_ASSERT(gyro_device != nullptr);
        STA_ASSERT(accel_device != nullptr);
    }

    bool BMI088::init()
    {
    	uint8_t id;
    	busRead(GYROSCOPE, BMI088_REG_GYRO_CHIP_ID, &id);
    	if(id != BMI088_GYRO_CHIP_ID) return false;

    	setGyroscopeRange(GyroRange::_1000);
    	setGyroscopeBandwidth(GyroBandwidth::_200_64);
    	setGyroscopeMode(GyroMode::NORMAL_AWAKE);

    	busRead(ACCELEROMETER, BMI088_REG_ACC_CHIP_ID, &id);
    	if(id != BMI088_ACC_CHIP_ID) return false;

    	setAccelerometerRange(AccelRange::_24G);
    	setAccelerometerBandwidth(AccelBandwidth::NORMAL_BANDWIDTH, AccelODR::_100);
    	setAccelerometerMode(AccelMode::ON);

    	return true;
    }

    void BMI088::setGyroscopeMode(GyroMode mode)
    {
    	busWrite(GYROSCOPE, BMI088_REG_GYRO_LPM1, mode);
    }
    void BMI088::setGyroscopeRange(GyroRange range)
    {
    	busWrite(GYROSCOPE, BMI088_REG_GYRO_RANGE, range);

    	switch(range)
    	{
    	case _2000:
    		f_gyro_range = 2000;
    		break;
    	case _1000:
    		f_gyro_range = 1000;
    		break;
    	case _500:
    		f_gyro_range = 500;
    		break;
    	case _250:
    		f_gyro_range = 250;
    		break;
    	case _125:
    		f_gyro_range = 125;
    		break;
    	default:
    		f_gyro_range = 0;
    		break;
    	}
    }
    void BMI088::setGyroscopeBandwidth(GyroBandwidth bandwidth)
    {
    	busWrite(GYROSCOPE, BMI088_REG_GYRO_BANDWIDTH, bandwidth);
    }
    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;

		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_LSB, &x_lsb);
		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_MSB, &x_msb);
		*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_MSB, &y_msb);
		*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_MSB, &z_msb);
		*z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);

    }
    void BMI088::getRotation(float* x, float* y, float* z)
    {
    	int16_t i_x,i_y,i_z;
		getRawRotation(&i_x, &i_y, &i_z);

		*x = lsbToDPS(i_x);
		*y = lsbToDPS(i_y);
		*z = lsbToDPS(i_z);
    }


    void BMI088::setAccelerometerMode(AccelMode mode)
    {
    	busWrite(ACCELEROMETER, BMI088_REG_ACC_PWR_CTRL, mode);
    }
    void BMI088::setAccelerometerRange(AccelRange range)
    {
    	busWrite(ACCELEROMETER, BMI088_REG_ACC_RANGE, range);

    	switch(range)
    	{
    	case _3G:
    		f_accel_range = 3*GRAVITY;
    		break;
    	case _6G:
    		f_accel_range = 6*GRAVITY;
    		break;
    	case _12G:
    		f_accel_range = 12*GRAVITY;
    		break;
    	case _24G:
    		f_accel_range = 24*GRAVITY;
    		break;
    	default:
    		break;
    	}
    }
    void BMI088::setAccelerometerBandwidth(AccelBandwidth bandwidth, AccelODR odr)
    {
    	uint8_t data = bandwidth<<4 | odr;//TODO are your sure? shift three or four times?
    	busWrite(ACCELEROMETER, BMI088_REG_ACC_CONF, data);
    }
    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;

		busRead(ACCELEROMETER, BMI088_REG_ACC_X_LSB, &x_lsb);
		busRead(ACCELEROMETER, BMI088_REG_ACC_X_MSB, &x_msb);
		*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_MSB, &y_msb);
		*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_MSB, &z_msb);
		*z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);

    }
    void BMI088::getAcceleration(float* x, float* y, float* z)
    {
    	int16_t i_x,i_y,i_z;
		getRawAcceleration(&i_x, &i_y, &i_z);

		*x = lsbToMps2(i_x);
		*y = lsbToMps2(i_y);
		*z = lsbToMps2(i_z);
    }



    bool BMI088::busRead(Part part, uint8_t reg, uint8_t * buffer)
    {
        if(part == GYROSCOPE)
        {
        	uint8_t data[1] = { (uint8_t)( reg | BMI088_READ_MASK ) };
        	gyro_device->beginTransmission();
        	gyro_device->transfer(data, 1);
        	gyro_device->receive(buffer, 1);
        	gyro_device->endTransmission();
        }
        if(part == ACCELEROMETER)
        {
        	uint8_t data[2] = { (uint8_t)( reg | BMI088_READ_MASK ), 0x00 };
        	accel_device->beginTransmission();
        	accel_device->transfer(data, 2);
        	accel_device->receive(buffer, 1);
        	accel_device->endTransmission();
        }
        return true;
    }

    bool BMI088::busWrite(Part part, uint8_t reg, uint8_t value)
    {
    	uint8_t data[2] = { reg, value };//apperently only supports writing 8bit at a time
		if(part == GYROSCOPE)
		{
			gyro_device->beginTransmission();
			gyro_device->transfer(data, 2);
			gyro_device->endTransmission();
		}
		if(part == ACCELEROMETER)
		{
			accel_device->beginTransmission();
			accel_device->transfer(data, 2);
			accel_device->endTransmission();
		}
		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){
		int16_t sign = (int16_t)((*i & 0b1000000000000000) >> 15);
		if(sign)
		{//negative
			uint16_t value = ~(*i) & 0b0111111111111111;
			*f = ((float)value / (float)0b0111111111111111) *-f_range;
		}
		else
		{//positive
			uint16_t value = *i & 0b0111111111111111;
			*f = ((float)value / (float)0b0111111111111111) *f_range;
		}
	}
} // namespace sta

#endif // STA_SPATZ_ENABLED