April 12, 2015
by scootergarrett
|
I finally got around to figuring out the adafruit 10DOF board, which forced me to learn I2C. So its a 3 axis accelerometer, 3 axis gyro (rotational velocity), 3 axis magnetometer, and has temperature and barometric pressure. I set it up with the 324p circuit diagram but the code should still work with a 328/168. I wrote functions that set up the I2C then set up the sensors set gains exc. Then I have functions that populate a structure IMURawData_S with space for each measurement. The pressure measurement is the strangest; first all the internal coefficients need to be read and stored from the sensor. Then the raw measurement needs to go through this crazy math just to get the barometric pressure.
I have been able to test the accelerometers and the magnetometer by mounting the sensor on a servo  motor and slowly rotating it some quic results:
And I tested the pressure measurments by driving up and down hills with it in my car.
bacause of the math required for the pressure using the pow() function I had to switch the make file line from:
avr-gcc ${GCCFLAGS} ${LINKFLAGS} -o $(ProgramName).o $(ProgramName).c ${LINKOBJECTS}
to:
avr-gcc ${GCCFLAGS} -o $(ProgramName).o $(ProgramName).c ${LINKFLAGS} ${LINKOBJECTS}
Here are all the functions I'm using/ developing:
/// How to get data from the Ada fruit 10DOF IMU ///
/// Still need pressure and rotational velocity and temp and structures and nice functions ///
struct IMURawData_S
{
uint16_t AccelerationX;
uint16_t AccelerationY;
uint16_t AccelerationZ;
uint16_t AngularVelocityX;
uint16_t AngularVelocityY;
uint16_t AngularVelocityZ;
uint16_t MagneticFieldX;
uint16_t MagneticFieldY;
uint16_t MagneticFieldZ;
uint32_t Pressure;
int16_t Temperature;
};
#define AccelerationSlaveAddress 0x32
#define Acceleration_CTRL_REG1_A_Register 0x20 // Enable and power mode
#define Acceleration_CTRL_REG4_A_Register 0x23 // Mode select and gain
#define Acceleration_OUT_X_L_A_Register 0x28 // X axes acceleration field Low bit
#define Acceleration_OUT_X_H_A_Register 0x29 // X axes acceleration field High bit
#define Acceleration_OUT_Y_L_A_Register 0x2A // Y axes acceleration field Low bit
#define Acceleration_OUT_Y_H_A_Register 0x2B // Y axes acceleration field High bit
#define Acceleration_OUT_Z_L_A_Register 0x2C // Z axes acceleration field Low bit
#define Acceleration_OUT_Z_H_A_Register 0x2D // Z axes acceleration field High bit
#define AngularVelocitySlaveAddress 0xD6
#define AngularVelocity_WHO_AM_I_Register 0x0F // Device identification register
#define AngularVelocity_WHO_AM_I_RegisterDefault 0xD7
#define AngularVelocity_CTRL1_Register 0x20 // Enable axis, bandwidth, and output data rate
#define AngularVelocity_OUT_X_L_Register 0x28 // X axes AngularVelocity Low bit
#define AngularVelocity_OUT_X_H_Register 0x29 // X axes AngularVelocity High bit
#define AngularVelocity_OUT_Y_L_Register 0x2A // Y axes AngularVelocity Low bit
#define AngularVelocity_OUT_Y_H_Register 0x2B // Y axes AngularVelocity High bit
#define AngularVelocity_OUT_Z_L_Register 0x2C // Z axes AngularVelocity Low bit
#define AngularVelocity_OUT_Z_H_Register 0x2D // Z axes AngularVelocity High bit
#define MagneticFieldSlaveAddress 0x3C
#define MagneticFieldS_CRA_REG_M_Register 0x00 // Temperature enable and DataRate
#define MagneticFieldS_CRB_REG_M_Register 0x01 // Gain Register
#define MagneticFieldS_MR_REG_M_Register 0x02 // Mode select
#define MagneticFieldS_OUT_X_H_M_Register 0x03 // X axes magnetic field High bit
#define MagneticFieldS_OUT_X_L_M_Register 0x04 // X axes magnetic field Low bit
#define MagneticFieldS_OUT_Z_H_M_Register 0x05 // Z axes magnetic field High bit
#define MagneticFieldS_OUT_Z_L_M_Register 0x06 // Z axes magnetic field Low bit
#define MagneticFieldS_OUT_Y_H_M_Register 0x07 // Y axes magnetic field High bit
#define MagneticFieldS_OUT_Y_L_M_Register 0x08 // Y axes magnetic field Low bit
#define PressureSlaveAddress 0xEE
#define BMP085_MODE_STANDARD 0x00
#define SCL_CLOCK 100000L
void I2C_INT();
uint8_t SetUpIMU();
uint8_t SendI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t Data);
uint8_t GetI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t *Data);
struct Bosch_Coefficients_S
{
int16_t AC1;
int16_t AC2;
int16_t AC3;
uint16_t AC4;
uint16_t AC5;
uint16_t AC6;
int16_t B1;
int16_t B2;
int16_t MB;
int16_t MC;
int16_t MD;
float BasePressure;
};
void Populate_Bosch_Coefficients(volatile struct Bosch_Coefficients_S *Bosch_Coefficients)
{
GetI2CData(PressureSlaveAddress, 0xAA, (uint8_t*)&Bosch_Coefficients->AC1 + 1 );
GetI2CData(PressureSlaveAddress, 0xAB, (uint8_t*)&Bosch_Coefficients->AC1 );
GetI2CData(PressureSlaveAddress, 0xAC, (uint8_t*)&Bosch_Coefficients->AC2 + 1 );
GetI2CData(PressureSlaveAddress, 0xAD, (uint8_t*)&Bosch_Coefficients->AC2 );
GetI2CData(PressureSlaveAddress, 0xAE, (uint8_t*)&Bosch_Coefficients->AC3 + 1 );
GetI2CData(PressureSlaveAddress, 0xAF, (uint8_t*)&Bosch_Coefficients->AC3 );
GetI2CData(PressureSlaveAddress, 0xB0, (uint8_t*)&Bosch_Coefficients->AC4 + 1 );
GetI2CData(PressureSlaveAddress, 0xB1, (uint8_t*)&Bosch_Coefficients->AC4 );
GetI2CData(PressureSlaveAddress, 0xB2, (uint8_t*)&Bosch_Coefficients->AC5 + 1 );
GetI2CData(PressureSlaveAddress, 0xB3, (uint8_t*)&Bosch_Coefficients->AC5 );
GetI2CData(PressureSlaveAddress, 0xB4, (uint8_t*)&Bosch_Coefficients->AC6 + 1 );
GetI2CData(PressureSlaveAddress, 0xB5, (uint8_t*)&Bosch_Coefficients->AC6 );
GetI2CData(PressureSlaveAddress, 0xB6, (uint8_t*)&Bosch_Coefficients->B1 + 1 );
GetI2CData(PressureSlaveAddress, 0xB7, (uint8_t*)&Bosch_Coefficients->B1 );
GetI2CData(PressureSlaveAddress, 0xB8, (uint8_t*)&Bosch_Coefficients->B2 + 1 );
GetI2CData(PressureSlaveAddress, 0xB9, (uint8_t*)&Bosch_Coefficients->B2 );
GetI2CData(PressureSlaveAddress, 0xBA, (uint8_t*)&Bosch_Coefficients->MB + 1 );
GetI2CData(PressureSlaveAddress, 0xBB, (uint8_t*)&Bosch_Coefficients->MB );
GetI2CData(PressureSlaveAddress, 0xBC, (uint8_t*)&Bosch_Coefficients->MC + 1 );
GetI2CData(PressureSlaveAddress, 0xBD, (uint8_t*)&Bosch_Coefficients->MC );
GetI2CData(PressureSlaveAddress, 0xBE, (uint8_t*)&Bosch_Coefficients->MD + 1 );
GetI2CData(PressureSlaveAddress, 0xBF, (uint8_t*)&Bosch_Coefficients->MD );
return;
}
volatile struct Bosch_Coefficients_S Bosch_Coefficients;
/// Sets up the I2C ///
void I2C_INT()
{
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; // [Bit rate register] controls signal speed p.232
TWCR |= (1<<TWEA) | (1<<TWEN) | (1<<TWINT); // [Control register] p.232
TWSR &= ~((1<<TWPS1) | (1<<TWPS0)); // [Status register 0-1] controls signal speed p.234
return;
}
uint8_t SendI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t Data)
{
/// Set start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x08)
return -1;
/// Send slave address + w ///
TWDR = SlaveAddress;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x18)
return -2;
/// Send sub address ///
TWDR = Register;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -3;
/// Send Data ///
TWDR = Data;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -4;
/// Stop bit ///
TWCR = (1<<TWINT) | (1<<TWSTO);
return 0;
}
uint8_t GetI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t *Data)
{
/// Set start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x08)
return -1;
/// Send slave address + w ///
TWDR = SlaveAddress;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x18)
// if ((TWSR & 0xF8) != 0x48)
return -2;
/// Send sub address ///
TWDR = Register;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -3;
/// Set repeat start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x10)
return -4;
/// Send slave address + r ///
TWDR = SlaveAddress | 0x01;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x40)
return -5;
/// Get data ///
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
/// Stop bit ///
TWCR = (1<<TWINT) | (1<<TWSTO);
*Data = TWDR;
return 0;
}
uint8_t SetUpIMU()
{
uint8_t RegisterData;
I2C_INT();
delay_ms(10);
/// Set up the Acceleration /// WIP need sensitivity set up
if( SendI2CData(AccelerationSlaveAddress, Acceleration_CTRL_REG1_A_Register, 0x57) )
return -1;
if( SendI2CData(AccelerationSlaveAddress, Acceleration_CTRL_REG4_A_Register, 0b00000000) ) /// Gain and High res mode????
return -1;
/// Set up the AngularVelocity /// WIP
if( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_WHO_AM_I_Register, &RegisterData) )
return -2;
if( RegisterData != AngularVelocity_WHO_AM_I_RegisterDefault)
return -2;
if( SendI2CData(AngularVelocitySlaveAddress, AngularVelocity_CTRL1_Register, 0b00001111) ) /// Enable stuff WIP (fast with high cut off freq?)
return -2;
/// Set up the MagneticField /// WIP need sensitivity set up
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_MR_REG_M_Register, 0b00000000) )
return -3;
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_CRA_REG_M_Register, 0b10011100) )
return -3;
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_CRB_REG_M_Register, 0b11100000) )
return -3;
/// Set up pressure measurements /// WIP
delay_ms(10);
Populate_Bosch_Coefficients(&Bosch_Coefficients);
return 0;
}
uint8_t IMUGetAcceleration(struct IMURawData_S IMURawData)
{
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_X_L_A_Register, (uint8_t)&((IMURawData).AccelerationX) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_X_H_A_Register, (uint8_t)&((*IMURawData).AccelerationX) + 1 ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Y_L_A_Register, (uint8_t*)&((*IMURawData).AccelerationY) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Y_H_A_Register, (uint8_t*)&((*IMURawData).AccelerationY) + 1 ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Z_L_A_Register, (uint8_t*)&((*IMURawData).AccelerationZ) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Z_H_A_Register, (uint8_t*)&((*IMURawData).AccelerationZ) + 1 ) )
return -1;
return 0;
}
uint8_t IMUGetMagneticField(struct IMURawData_S *IMURawData)
{
/// Looks if the data is new /// WIP
// if ( GetI2CData(MagneticFieldSlaveAddress, 0x09, &k) )
// return -1;
// if(!(k & 0x01))
// continue;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_X_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldX) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_X_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldX) ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Z_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldZ) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Z_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldZ) ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Y_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldY) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Y_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldY) ) )
return -1;
return 0;
}
uint8_t IMUGAngularVelocity(struct IMURawData_S *IMURawData)
{
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_X_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityX) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_X_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityX) + 1 ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Y_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityY) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Y_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityY) + 1 ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Z_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityZ) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Z_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityZ) + 1 ) )
return -1;
return 0;
}
uint8_t IMUPressure(struct IMURawData_S *IMURawData)
{
int32_t UT = 0, UP = 0, X1 = 0, X2 = 0, B5 = 0, B6 = 0, X3 = 0, B3 = 0, p = 0;
uint32_t B4 = 0, B7 = 0;
uint8_t MSB, LSB, XLSB;
/// Get the Raw temperature ///
SendI2CData(PressureSlaveAddress, 0xF4, 0x2E);
delay_ms(5);
GetI2CData(PressureSlaveAddress, 0xF6, (uint8_t*)&UT + 1 );
GetI2CData(PressureSlaveAddress, 0xF7, (uint8_t*)&UT );
/// Get the raw pressure //
SendI2CData(PressureSlaveAddress, 0xF4, 0x34 + (BMP085_MODE_STANDARD<<6) );
delay_ms(30);
GetI2CData(PressureSlaveAddress, 0xF6, &MSB);
GetI2CData(PressureSlaveAddress, 0xF7, &LSB);
GetI2CData(PressureSlaveAddress, 0xF8, &XLSB);
UP = ( (((int32_t)MSB)<<16) + (((uint32_t)LSB)<<8) + XLSB) >> (8-BMP085_MODE_STANDARD);
/// Calculate the temperatur ///
X1 = (UT - Bosch_Coefficients.AC6)*Bosch_Coefficients.AC5 / 32768L;
X2 = ((int32_t)Bosch_Coefficients.MC << 11) / (X1 + (int32_t)Bosch_Coefficients.MD);
B5 = X1 + X2;
IMURawData->Temperature = (float)((B5 + 8) >> 4);
/// Calculate the pressure ///
B6 = B5 - 4000;
X1 = (Bosch_Coefficients.B2 * ((B6 * B6) >> 12)) >> 11;
X2 = (Bosch_Coefficients.AC2 * B6) >> 11;
X3 = X1 + X2;
B3 = ((((int32_t)Bosch_Coefficients.AC1 * 4 + X3) << BMP085_MODE_STANDARD) + 2) >> 2;
X1 = (Bosch_Coefficients.AC3 * B6) >> 13;
X2 = (Bosch_Coefficients.B1 * ((B6 * B6) >> 12)) >> 16;
X3 = ((X1 + X2) + 2) >> 2;
B4 = (Bosch_Coefficients.AC4 * (uint32_t) (X3 + 32768)) >> 15;
B7 = ( ((uint32_t)UP - B3) * (uint32_t)(50000 >> BMP085_MODE_STANDARD));
if (B7 < 0x80000000)
p = (B7 << 1) / B4;
else
p = (B7 / B4) << 1;
X1 = (p >> 8) * (p >> 8);
X1 = (X1 * 3038) >> 16;
X2 = (-7357 * p) >> 16;
IMURawData->Pressure = p + ((X1 + X2 + 3791) >> 4);
return 0;
}
float CalculateAltitude(uint32_t Pressure)
{
float Alt;
// Alt = 44330 * ( 1- pow( (float)Pressure/101325.0 , 0.190294957183635 ) );
Alt = 44330 * ( 1- pow( (float)Pressure/Bosch_Coefficients.BasePressure , 0.190294957183635 ) );
return Alt;
}
void CalSeaLevelPressure(uint16_t Starting_Elevation)
{
struct IMURawData_S IMURawData;
IMUPressure(&IMURawData);
Bosch_Coefficients.BasePressure = IMURawData.Pressure / pow(1.0 - (Starting_Elevation/44330.0), 5.255);
return;
}
and here is a quick code that shows current pressure on the LCD:
#define TestPin A,0
int main()
{
struct IMURawData_S IMURawData;
int8_t k = 0;
float Altitude;
OUTPUT(TestPin);
lcd_init();
fdev_setup_stream(&lcd_stream, lcd_putchar, 0, _FDEV_SETUP_WRITE);
lcd_clear_and_home();
uart_init();
if(SetUpIMU())
goto error;
while(true)
{
IMUPressure(&IMURawData);
lcd_line_one();
fprintf_P(&lcd_stream, PSTR("P: %li "), IMURawData.Pressure);
lcd_line_two();
fprintf_P(&lcd_stream, PSTR("T: %.1f "), (float)IMURawData.Temperature/10);
delay_ms(1000);
}
error:
fprintf_P(&lcd_stream, PSTR("ERROR"));
lcd_line_two();
fprintf_P(&lcd_stream, PSTR("TWSR & 0xF8: %X "), TWSR & 0xF8 );
lcd_line_three();
fprintf_P(&lcd_stream, PSTR("k: %i "), k);
while(1)
{
TOGGLE(TestPin);
delay_ms(100);
}
return 0;
}
I'm not using the interrupt based I2C yet maybe I will try that as some point, but it doesn’t seem to take that much processing time to take measurements |
April 12, 2015
by scootergarrett
|
I don't know why the code got all broken up there I will try again
/// How to get data from the Ada fruit 10DOF IMU ///
/// Still need pressure and rotational velocity and temp and structures and nice functions ///
struct IMURawData_S
{
uint16_t AccelerationX;
uint16_t AccelerationY;
uint16_t AccelerationZ;
uint16_t AngularVelocityX;
uint16_t AngularVelocityY;
uint16_t AngularVelocityZ;
uint16_t MagneticFieldX;
uint16_t MagneticFieldY;
uint16_t MagneticFieldZ;
uint32_t Pressure;
int16_t Temperature;
};
#define AccelerationSlaveAddress 0x32
#define Acceleration_CTRL_REG1_A_Register 0x20 // Enable and power mode
#define Acceleration_CTRL_REG4_A_Register 0x23 // Mode select and gain
#define Acceleration_OUT_X_L_A_Register 0x28 // X axes acceleration field Low bit
#define Acceleration_OUT_X_H_A_Register 0x29 // X axes acceleration field High bit
#define Acceleration_OUT_Y_L_A_Register 0x2A // Y axes acceleration field Low bit
#define Acceleration_OUT_Y_H_A_Register 0x2B // Y axes acceleration field High bit
#define Acceleration_OUT_Z_L_A_Register 0x2C // Z axes acceleration field Low bit
#define Acceleration_OUT_Z_H_A_Register 0x2D // Z axes acceleration field High bit
#define AngularVelocitySlaveAddress 0xD6
#define AngularVelocity_WHO_AM_I_Register 0x0F // Device identification register
#define AngularVelocity_WHO_AM_I_RegisterDefault 0xD7
#define AngularVelocity_CTRL1_Register 0x20 // Enable axis, bandwidth, and output data rate
#define AngularVelocity_OUT_X_L_Register 0x28 // X axes AngularVelocity Low bit
#define AngularVelocity_OUT_X_H_Register 0x29 // X axes AngularVelocity High bit
#define AngularVelocity_OUT_Y_L_Register 0x2A // Y axes AngularVelocity Low bit
#define AngularVelocity_OUT_Y_H_Register 0x2B // Y axes AngularVelocity High bit
#define AngularVelocity_OUT_Z_L_Register 0x2C // Z axes AngularVelocity Low bit
#define AngularVelocity_OUT_Z_H_Register 0x2D // Z axes AngularVelocity High bit
#define MagneticFieldSlaveAddress 0x3C
#define MagneticFieldS_CRA_REG_M_Register 0x00 // Temperature enable and DataRate
#define MagneticFieldS_CRB_REG_M_Register 0x01 // Gain Register
#define MagneticFieldS_MR_REG_M_Register 0x02 // Mode select
#define MagneticFieldS_OUT_X_H_M_Register 0x03 // X axes magnetic field High bit
#define MagneticFieldS_OUT_X_L_M_Register 0x04 // X axes magnetic field Low bit
#define MagneticFieldS_OUT_Z_H_M_Register 0x05 // Z axes magnetic field High bit
#define MagneticFieldS_OUT_Z_L_M_Register 0x06 // Z axes magnetic field Low bit
#define MagneticFieldS_OUT_Y_H_M_Register 0x07 // Y axes magnetic field High bit
#define MagneticFieldS_OUT_Y_L_M_Register 0x08 // Y axes magnetic field Low bit
#define PressureSlaveAddress 0xEE
#define BMP085_MODE_STANDARD 0x00
#define SCL_CLOCK 100000L
void I2C_INT();
uint8_t SetUpIMU();
uint8_t SendI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t Data);
uint8_t GetI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t *Data);
struct Bosch_Coefficients_S
{
int16_t AC1;
int16_t AC2;
int16_t AC3;
uint16_t AC4;
uint16_t AC5;
uint16_t AC6;
int16_t B1;
int16_t B2;
int16_t MB;
int16_t MC;
int16_t MD;
float BasePressure;
};
void Populate_Bosch_Coefficients(volatile struct Bosch_Coefficients_S *Bosch_Coefficients)
{
GetI2CData(PressureSlaveAddress, 0xAA, (uint8_t*)&Bosch_Coefficients->AC1 + 1 );
GetI2CData(PressureSlaveAddress, 0xAB, (uint8_t*)&Bosch_Coefficients->AC1 );
GetI2CData(PressureSlaveAddress, 0xAC, (uint8_t*)&Bosch_Coefficients->AC2 + 1 );
GetI2CData(PressureSlaveAddress, 0xAD, (uint8_t*)&Bosch_Coefficients->AC2 );
GetI2CData(PressureSlaveAddress, 0xAE, (uint8_t*)&Bosch_Coefficients->AC3 + 1 );
GetI2CData(PressureSlaveAddress, 0xAF, (uint8_t*)&Bosch_Coefficients->AC3 );
GetI2CData(PressureSlaveAddress, 0xB0, (uint8_t*)&Bosch_Coefficients->AC4 + 1 );
GetI2CData(PressureSlaveAddress, 0xB1, (uint8_t*)&Bosch_Coefficients->AC4 );
GetI2CData(PressureSlaveAddress, 0xB2, (uint8_t*)&Bosch_Coefficients->AC5 + 1 );
GetI2CData(PressureSlaveAddress, 0xB3, (uint8_t*)&Bosch_Coefficients->AC5 );
GetI2CData(PressureSlaveAddress, 0xB4, (uint8_t*)&Bosch_Coefficients->AC6 + 1 );
GetI2CData(PressureSlaveAddress, 0xB5, (uint8_t*)&Bosch_Coefficients->AC6 );
GetI2CData(PressureSlaveAddress, 0xB6, (uint8_t*)&Bosch_Coefficients->B1 + 1 );
GetI2CData(PressureSlaveAddress, 0xB7, (uint8_t*)&Bosch_Coefficients->B1 );
GetI2CData(PressureSlaveAddress, 0xB8, (uint8_t*)&Bosch_Coefficients->B2 + 1 );
GetI2CData(PressureSlaveAddress, 0xB9, (uint8_t*)&Bosch_Coefficients->B2 );
GetI2CData(PressureSlaveAddress, 0xBA, (uint8_t*)&Bosch_Coefficients->MB + 1 );
GetI2CData(PressureSlaveAddress, 0xBB, (uint8_t*)&Bosch_Coefficients->MB );
GetI2CData(PressureSlaveAddress, 0xBC, (uint8_t*)&Bosch_Coefficients->MC + 1 );
GetI2CData(PressureSlaveAddress, 0xBD, (uint8_t*)&Bosch_Coefficients->MC );
GetI2CData(PressureSlaveAddress, 0xBE, (uint8_t*)&Bosch_Coefficients->MD + 1 );
GetI2CData(PressureSlaveAddress, 0xBF, (uint8_t*)&Bosch_Coefficients->MD );
return;
}
volatile struct Bosch_Coefficients_S Bosch_Coefficients;
/// Sets up the I2C ///
void I2C_INT()
{
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; // [Bit rate register] controls signal speed p.232
TWCR |= (1<<TWEA) | (1<<TWEN) | (1<<TWINT); // [Control register] p.232
TWSR &= ~((1<<TWPS1) | (1<<TWPS0)); // [Status register 0-1] controls signal speed p.234
return;
}
uint8_t SendI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t Data)
{
/// Set start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x08)
return -1;
/// Send slave address + w ///
TWDR = SlaveAddress;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x18)
return -2;
/// Send sub address ///
TWDR = Register;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -3;
/// Send Data ///
TWDR = Data;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -4;
/// Stop bit ///
TWCR = (1<<TWINT) | (1<<TWSTO);
return 0;
}
uint8_t GetI2CData(uint8_t SlaveAddress, uint8_t Register, uint8_t *Data)
{
/// Set start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x08)
return -1;
/// Send slave address + w ///
TWDR = SlaveAddress;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x18)
// if ((TWSR & 0xF8) != 0x48)
return -2;
/// Send sub address ///
TWDR = Register;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)));
if ((TWSR & 0xF8) != 0x28)
return -3;
/// Set repeat start ///
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x10)
return -4;
/// Send slave address + r ///
TWDR = SlaveAddress | 0x01;
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
if ((TWSR & 0xF8) != 0x40)
return -5;
/// Get data ///
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT))); // wait
/// Stop bit ///
TWCR = (1<<TWINT) | (1<<TWSTO);
*Data = TWDR;
return 0;
}
uint8_t SetUpIMU()
{
uint8_t RegisterData;
I2C_INT();
delay_ms(10);
/// Set up the Acceleration /// WIP need sensitivity set up
if( SendI2CData(AccelerationSlaveAddress, Acceleration_CTRL_REG1_A_Register, 0x57) )
return -1;
if( SendI2CData(AccelerationSlaveAddress, Acceleration_CTRL_REG4_A_Register, 0b00000000) ) /// Gain and High res mode????
return -1;
/// Set up the AngularVelocity /// WIP
if( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_WHO_AM_I_Register, &RegisterData) )
return -2;
if( RegisterData != AngularVelocity_WHO_AM_I_RegisterDefault)
return -2;
if( SendI2CData(AngularVelocitySlaveAddress, AngularVelocity_CTRL1_Register, 0b00001111) ) /// Enable stuff WIP (fast with high cut off freq?)
return -2;
/// Set up the MagneticField /// WIP need sensitivity set up
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_MR_REG_M_Register, 0b00000000) )
return -3;
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_CRA_REG_M_Register, 0b10011100) )
return -3;
if( SendI2CData(MagneticFieldSlaveAddress, MagneticFieldS_CRB_REG_M_Register, 0b11100000) )
return -3;
/// Set up pressure measurements /// WIP
delay_ms(10);
Populate_Bosch_Coefficients(&Bosch_Coefficients);
return 0;
}
uint8_t IMUGetAcceleration(struct IMURawData_S *IMURawData)
{
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_X_L_A_Register, (uint8_t*)&((*IMURawData).AccelerationX) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_X_H_A_Register, (uint8_t*)&((*IMURawData).AccelerationX) + 1 ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Y_L_A_Register, (uint8_t*)&((*IMURawData).AccelerationY) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Y_H_A_Register, (uint8_t*)&((*IMURawData).AccelerationY) + 1 ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Z_L_A_Register, (uint8_t*)&((*IMURawData).AccelerationZ) ) )
return -1;
if ( GetI2CData(AccelerationSlaveAddress, Acceleration_OUT_Z_H_A_Register, (uint8_t*)&((*IMURawData).AccelerationZ) + 1 ) )
return -1;
return 0;
}
uint8_t IMUGetMagneticField(struct IMURawData_S *IMURawData)
{
/// Looks if the data is new /// WIP
// if ( GetI2CData(MagneticFieldSlaveAddress, 0x09, &k) )
// return -1;
// if(!(k & 0x01))
// continue;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_X_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldX) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_X_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldX) ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Z_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldZ) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Z_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldZ) ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Y_H_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldY) + 1 ) )
return -1;
if ( GetI2CData(MagneticFieldSlaveAddress, MagneticFieldS_OUT_Y_L_M_Register, (uint8_t*)&((*IMURawData).MagneticFieldY) ) )
return -1;
return 0;
}
uint8_t IMUGAngularVelocity(struct IMURawData_S *IMURawData)
{
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_X_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityX) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_X_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityX) + 1 ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Y_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityY) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Y_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityY) + 1 ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Z_L_Register, (uint8_t*)&((*IMURawData).AngularVelocityZ) ) )
return -1;
if ( GetI2CData(AngularVelocitySlaveAddress, AngularVelocity_OUT_Z_H_Register, (uint8_t*)&((*IMURawData).AngularVelocityZ) + 1 ) )
return -1;
return 0;
}
uint8_t IMUPressure(struct IMURawData_S *IMURawData)
{
int32_t UT = 0, UP = 0, X1 = 0, X2 = 0, B5 = 0, B6 = 0, X3 = 0, B3 = 0, p = 0;
uint32_t B4 = 0, B7 = 0;
uint8_t MSB, LSB, XLSB;
/// Get the Raw temperature ///
SendI2CData(PressureSlaveAddress, 0xF4, 0x2E);
delay_ms(5);
GetI2CData(PressureSlaveAddress, 0xF6, (uint8_t*)&UT + 1 );
GetI2CData(PressureSlaveAddress, 0xF7, (uint8_t*)&UT );
/// Get the raw pressure //
SendI2CData(PressureSlaveAddress, 0xF4, 0x34 + (BMP085_MODE_STANDARD<<6) );
delay_ms(30);
GetI2CData(PressureSlaveAddress, 0xF6, &MSB);
GetI2CData(PressureSlaveAddress, 0xF7, &LSB);
GetI2CData(PressureSlaveAddress, 0xF8, &XLSB);
UP = ( (((int32_t)MSB)<<16) + (((uint32_t)LSB)<<8) + XLSB) >> (8-BMP085_MODE_STANDARD);
/// Calculate the temperatur ///
X1 = (UT - Bosch_Coefficients.AC6)*Bosch_Coefficients.AC5 / 32768L;
X2 = ((int32_t)Bosch_Coefficients.MC << 11) / (X1 + (int32_t)Bosch_Coefficients.MD);
B5 = X1 + X2;
IMURawData->Temperature = (float)((B5 + 8) >> 4);
/// Calculate the pressure ///
B6 = B5 - 4000;
X1 = (Bosch_Coefficients.B2 * ((B6 * B6) >> 12)) >> 11;
X2 = (Bosch_Coefficients.AC2 * B6) >> 11;
X3 = X1 + X2;
B3 = ((((int32_t)Bosch_Coefficients.AC1 * 4 + X3) << BMP085_MODE_STANDARD) + 2) >> 2;
X1 = (Bosch_Coefficients.AC3 * B6) >> 13;
X2 = (Bosch_Coefficients.B1 * ((B6 * B6) >> 12)) >> 16;
X3 = ((X1 + X2) + 2) >> 2;
B4 = (Bosch_Coefficients.AC4 * (uint32_t) (X3 + 32768)) >> 15;
B7 = ( ((uint32_t)UP - B3) * (uint32_t)(50000 >> BMP085_MODE_STANDARD));
if (B7 < 0x80000000)
p = (B7 << 1) / B4;
else
p = (B7 / B4) << 1;
X1 = (p >> 8) * (p >> 8);
X1 = (X1 * 3038) >> 16;
X2 = (-7357 * p) >> 16;
IMURawData->Pressure = p + ((X1 + X2 + 3791) >> 4);
return 0;
}
float CalculateAltitude(uint32_t Pressure)
{
float Alt;
// Alt = 44330 * ( 1- pow( (float)Pressure/101325.0 , 0.190294957183635 ) );
Alt = 44330 * ( 1- pow( (float)Pressure/Bosch_Coefficients.BasePressure , 0.190294957183635 ) );
return Alt;
}
void CalSeaLevelPressure(uint16_t Starting_Elevation)
{
struct IMURawData_S IMURawData;
IMUPressure(&IMURawData);
Bosch_Coefficients.BasePressure = IMURawData.Pressure / pow(1.0 - (Starting_Elevation/44330.0), 5.255);
return;
}
|
