File content as of revision 1:da3b20b5d38a:

/* This file is part of the Razor AHRS Firmware */
#include "AHRS.h"

// I2C code to read the sensors

// Sensor I2C addresses
#define ACCEL_READ  0xA7
#define ACCEL_WRITE 0xA6
#define MAGN_WRITE  0x3C
#define MAGN_READ   0x3D
#define GYRO_WRITE  0xD0
#define GYRO_READ   0xD1

void IMU::I2C_Init()
{
    Wire.frequency(100000);
}

void IMU::Accel_Init()
{
    char tx[2];

    tx[0] = 0x2D; // Power register
    tx[1] = 0x08; // Power register
    Wire.write(ACCEL_WRITE, tx, 2);
    wait_ms(5);

    tx[0] = 0x31; // Data format register
    tx[1] = 0x08; // Set to full resolution
    Wire.write(ACCEL_WRITE, tx, 2);
    wait_ms(5);

    // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
    tx[0] = 0x2C;  // Rate
    tx[1] = 0x09;  // Set to 50Hz, normal operation
    Wire.write(ACCEL_WRITE, tx, 2);
    wait_ms(5);
}

// Reads x, y and z accelerometer registers
void IMU::Read_Accel()
{
    char buff[6];
    char tx = 0x32;  // Send address to read from

    Wire.write(ACCEL_WRITE, &tx, 1);

    if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received?
        // No multiply by -1 for coordinate system transformation here, because of double negation:
        // We want the gravity vector, which is negated acceleration vector.
        accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis)
        accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis)
        accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis)
    } else {
        num_accel_errors++;
        if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
    }
}

void IMU::Magn_Init()
{
    char tx[2];
    tx[0] = 0x02; // Mode
    tx[1] = 0x00; // Set continuous mode (default 10Hz)
    Wire.write(MAGN_WRITE, tx, 2);
    wait_ms(5);

    tx[0] = 0x00; // CONFIG_A
    tx[1] = 0x18; // Set 75Hz
    Wire.write(MAGN_WRITE, tx, 2);
    wait_ms(5);
}

void IMU::Read_Magn()
{
    char buff[6];
    char tx = 0x03;  // Send address to read from

    Wire.write(MAGN_WRITE, &tx, 1);

    if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received?
// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
#if HW__VERSION_CODE == 10125
        // MSB byte first, then LSB; X, Y, Z
        magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3];  // X axis (internal sensor -y axis)
        magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
        magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
#elif HW__VERSION_CODE == 10736
        // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
        magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5];  // X axis (internal sensor -y axis)
        magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
        magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3];  // Z axis (internal sensor -z axis)
// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
        // MSB byte first, then LSB; X, Y, Z
        magnetom[0] = (((int) buff[0]) << 8) | buff[1];       // X axis (internal sensor x axis)
        magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3];  // Y axis (internal sensor -y axis)
        magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
#elif HW__VERSION_CODE == 10724
        // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
        magnetom[0] =  1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis)
        magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis)
        magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis)
#endif
    } else {
        num_magn_errors++;
        if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
    }
}

void IMU::Gyro_Init()
{
    char tx[2];

    // Power up reset defaults
    tx[0] = 0x3E; // Power management
    tx[1] = 0x80; // ?
    Wire.write(GYRO_WRITE, tx, 2);
    wait_ms(5);

    // Select full-scale range of the gyro sensors
    // Set LP filter bandwidth to 42Hz
    tx[0] = 0x16; //
    tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
    Wire.write(GYRO_WRITE, tx, 2);
    wait_ms(5);

    // Set sample rato to 50Hz
    tx[0] = 0x15; //
    tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
    Wire.write(GYRO_WRITE, tx, 2);
    wait_ms(5);

    // Set clock to PLL with z gyro reference
    tx[0] = 0x3E;
    tx[1] = 0x00;
    Wire.write(GYRO_WRITE, tx, 2);
    wait_ms(5);
    
    // Set offset values
    foffset[0] = 99.5; // x (standard axis)
    foffset[1] = -45.0; // y
    foffset[2] = -29.7; // z
    // set slope values
    slope[0] = -1.05;
    slope[1] = 0.95;
    slope[2] = 0.47;
}

// Reads x, y and z gyroscope registers - and temperature
void IMU::Read_Gyro()
{
    char buff[8];
    int16_t readings[3];
    char tx = 0x1B; // Sends address to read from

    Wire.write(GYRO_WRITE, &tx, 1);

    if (Wire.read(GYRO_READ, buff, 8) == 0) { // All bytes received?
        int16_t temp = int16_t(((unsigned char)buff[0] << 8) | (unsigned char)buff[1]);
        temperature = 35.0 + ((temp + 13200)/280.0); // temperature
        for(int i = 0; i < 3; i++) {
            readings[i] = (buff[(i*2)+2] << 8) | (buff[(i*2)+3]);
            readings[i] -= slope[i] * temperature + foffset[i];
        }
        gyro[0] = -1 * readings[1];   // X axis (internal sensor -y axis)
        gyro[1] = -1 * readings[0];   // Y axis (internal sensor -x axis)
        gyro[2] = -1 * readings[2];   // Z axis (internal sensor -z axis)
    } else {
        num_gyro_errors++;
        if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
    }
}