| 0,0 → 1,108 |
|---|
| #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]; |
| } |
| } |