Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
#include "defines.h"
#include "sensor_accel_LSM303.h"
#include "base_I2C.h"
static LSM303_DATA *lsm303_data_p;
void LSM303_Init(LSM303_DATA* data, char device, char sa0) {
lsm303_data_p = data;
lsm303_data_p->device = device;
switch (device) {
case LSM303DLH_DEVICE:
case LSM303DLM_DEVICE:
if (sa0 == LSM303_SA0_A_LOW)
lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_LOW;
else
lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_HIGH;
break;
case LSM303DLHC_DEVICE:
lsm303_data_p->acc_address = ACC_ADDRESS_SA0_A_HIGH;
break;
default:
break;
}
}
void LSM303_Begin() {
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
LSM303_Write_A_Reg(LSM303_CTRL_REG1_A, 0x27);
// Enable Magnetometer
// 0x00 = 0b00000000
// Continuous conversion mode
LSM303_Write_M_Reg(LSM303_MR_REG_M, 0x00);
}
void LSM303_Write_A_Reg(char reg, char value) {
char buffer[2];
buffer[0] = reg;
buffer[1] = value;
I2C_Master_Send(lsm303_data_p->acc_address, 2, buffer);
char result;
do {
result = I2C_Get_Status();
} while (!result);
}
void LSM303_Write_M_Reg(char reg, char value) {
char buffer[2];
buffer[0] = reg;
buffer[1] = value;
I2C_Master_Send(lsm303_data_p->mag_address, 2, buffer);
char result;
do {
result = I2C_Get_Status();
} while (!result);
}
void LSM303_Set_Mag_Gain(enum magGain value) {
char buffer[2];
buffer[0] = LSM303_CRB_REG_M;
buffer[1] = (char)value;
I2C_Master_Send(lsm303_data_p->mag_address, 2, buffer);
char result;
do {
result = I2C_Get_Status();
} while (!result);
}
void LSM303_Read_Acc(int* x, int* y, int* z) {
char buffer[6];
char value = LSM303_OUT_X_L_A | 0x80;
I2C_Master_Restart(lsm303_data_p->acc_address, value, 6);
char result;
do {
result = I2C_Get_Status();
} while (!result);
I2C_Read_Buffer(buffer);
// 0 = x_l, 1 = x_h, 2 = y_l, ...
*x = buffer[1] << 8 | buffer[0];
*y = buffer[3] << 8 | buffer[2];
*z = buffer[5] << 8 | buffer[4];
}
void LSM303_Read_Mag(int* x, int* y, int* z) {
char buffer[6];
char value = LSM303_OUT_X_H_M;
I2C_Master_Restart(lsm303_data_p->mag_address, value, 6);
char result;
do {
result = I2C_Get_Status();
} while (!result);
I2C_Read_Buffer(buffer);
*x = buffer[0] << 8 | buffer[1];
if (lsm303_data_p->device == LSM303DLH_DEVICE) {
// DLH: register address for Y comes before Z
*y = buffer[2] << 8 | buffer[3];
*z = buffer[4] << 8 | buffer[5];
} else {
// DLM, DLHC: register address for Z comes before Y
*z = buffer[2] << 8 | buffer[3];
*y = buffer[4] << 8 | buffer[5];
}
}