Subversion Repositories Code-Repo

#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];
    }
}