| 0,0 → 1,81 |
|---|
| #include <xc.h> |
| #include "main.h" |
| #include "LSM303.h" |
| #include "I2C.h" |
| |
| void LSM303_Init(void) { |
| // Enable accelerometer, normal power mode, all axes enabled (100Hz) |
| char c[2] = {LSM303_CTRL_REG1_A, 0x57}; |
| I2C_Master_Send(ADDRESS_ACCL, &c, 2); |
| while (!I2C_Get_Status()); |
| |
| // Enable high resolution mode, +- 2G |
| char d[2] = {LSM303_CTRL_REG4_A, 0x08}; |
| I2C_Master_Send(ADDRESS_ACCL, &d, 2); |
| while (!I2C_Get_Status()); |
| |
| // Enable magnetometer, continuous conversion mode |
| char e[2] = {LSM303_MR_REG_M, 0x00}; |
| I2C_Master_Send(ADDRESS_MAGN, &e, 2); |
| while (!I2C_Get_Status()); |
| |
| char f[2] = {LSM303_CRA_REG_M, 0x18}; // (75Hz) |
| I2C_Master_Send(ADDRESS_MAGN, &f, 2); |
| while (!I2C_Get_Status()); |
| |
| // LSM303_Set_Mag_Gain(magGain_40); |
| } |
| |
| void LSM303_Set_Mag_Gain(magGain value) { |
| char c[2]; |
| c[0] = LSM303_CRB_REG_M; |
| c[1] = (char)value; |
| I2C_Master_Send(ADDRESS_MAGN, &c, 2); |
| while (!I2C_Get_Status()); |
| } |
| |
| void LSM303_Read_Accl(char *x_L, char *x_H, char *y_L, char *y_H, char *z_L, char *z_H) { |
| char result, length, buffer[10] = {0}; |
| |
| // Assert the MSB to autoincrement address |
| char c = LSM303_OUT_X_L_A | 0x80; |
| |
| I2C_Master_Restart(ADDRESS_ACCL, c, 6); |
| do { |
| result = I2C_Get_Status(); |
| } while (!result); |
| |
| // Read received data |
| length = I2C_Read_Buffer(buffer); |
| |
| // Data is received in low-high byte order |
| *x_L = buffer[0]; |
| *x_H = buffer[1]; |
| *y_L = buffer[2]; |
| *y_H = buffer[3]; |
| *z_L = buffer[4]; |
| *z_H = buffer[5]; |
| } |
| |
| void LSM303_Read_Magn(char *x_L, char *x_H, char *y_L, char *y_H, char *z_L, char *z_H) { |
| char result, length, buffer[10]; |
| |
| // Assert the MSB to autoincrement address |
| char c = LSM303_OUT_X_H_M; |
| |
| I2C_Master_Restart(ADDRESS_MAGN, c, 6); |
| do { |
| result = I2C_Get_Status(); |
| } while (!result); |
| |
| // Read received data |
| length = I2C_Read_Buffer(buffer); |
| |
| // Data is received in high-low byte order |
| *x_H = buffer[0]; |
| *x_L = buffer[1]; |
| *z_H = buffer[2]; |
| *z_L = buffer[3]; |
| *y_H = buffer[4]; |
| *y_L = buffer[5]; |
| } |