IMU3000 class

/*
 * IMU3000.cpp
 *
 *  Created on: 27 mai 2012
 *      Author: blackswords
 */

#include "IMU3000.h"

HardWire *IMUWire;

IMU3000 IMU;

IMU3000::IMU3000() {

    FS = DEG_2000;

    LP = F_256Hz;

}

void IMU3000::Init(uint8 I2CPort, uint8 flags) {

    IMUWire = new HardWire(I2CPort, flags);

    IMUWire->begin();

    //Enable pass-through
    WriteReg(IMU3000_ADDR, USER_CTRL, 0x08);

    // Configure the ADXL345
    WriteReg(ADXL345_ADDR, POWER_CTL,0x08);

    // Set PLL with Gyro X reference
    WriteReg(IMU3000_ADDR, PWR_MGM, 0x01);

    // Full scale range & Low Pass Filter
    WriteReg(IMU3000_ADDR, DLPF_FS, FS << 3 | LP);

    calib.x = 0;
    calib.y = 0;
    calib.z = 0;

}

IMU3000::~IMU3000() {
    IMUWire->~HardWire();
}

void IMU3000::Update(void) {
    GetGyro();
    GetAcc();
}

void IMU3000::GetGyro() {
    uint8_t buff[6];

    BurstRead(IMU3000_ADDR, IMU3000_BURST_R_ADDR, 6, buff);

#ifdef AUTO_CALIB
    gyro.x = (int16_t)(buff[0]<<8 | buff[1]);
    gyro.y = (int16_t)(buff[2]<<8 | buff[3]);
    gyro.z = (int16_t)(buff[4]<<8 | buff[5]);
#else
    gyro.x = (int16_t)(buff[0]<<8 | buff[1]) + calib.x;
    gyro.y = (int16_t)(buff[2]<<8 | buff[3]) + calib.y;
    gyro.z = (int16_t)(buff[4]<<8 | buff[5]) + calib.z;
#endif
}

void IMU3000::GetAcc() {

    BurstRead(ADXL345_ADDR, ADXL345_BURST_R_ADDR, 6, (uint8_t*)&acc);

}

void IMU3000::SetFullScaleRange(fullScale new_fs) {
    FS = new_fs;

    // Full scale range
    WriteReg(IMU3000_ADDR, DLPF_FS, FS << 3 | LP);
}

void IMU3000::SetLowPassFilter(LPFilter new_lp) {
    LP = new_lp;

    // Low Pass Filter
    WriteReg(IMU3000_ADDR, DLPF_FS, FS << 3 | LP);
}

void IMU3000::Calibration() {
#ifdef AUTO_CALIB

    int32_t cal[3]={0};
    uint8_t i;

    for(i = 0; i< 10; i++) {
        GetGyro();
        cal[0] += gyro.x;
        cal[1] += gyro.y;
        cal[2] += gyro.z;

        delay(200);
    }

    calib.x = 0xFFFF - cal[0] / 10 + 1;
    calib.y = 0xFFFF - cal[1] / 10 + 1;
    calib.z = 0xFFFF - cal[2] / 10 + 1;

    char s[30];

    sprintf(s,"Cal  %5d %5d %5d",IMU.calib.x,IMU.calib.y,IMU.calib.z);
    LCD::WriteText(10,110,s,Red,Black);

    WriteReg(IMU3000_ADDR, X_OFFS_USRH,(calib.x & 0xFF00) >> 8);
    WriteReg(IMU3000_ADDR, X_OFFS_USRL,(calib.x & 0x00FF));

    WriteReg(IMU3000_ADDR, Y_OFFS_USRH,(calib.y & 0xFF00) >> 8);
    WriteReg(IMU3000_ADDR, Y_OFFS_USRL,(calib.y & 0x00FF));

    WriteReg(IMU3000_ADDR, Z_OFFS_USRH,(calib.z & 0xFF00) >> 8);
    WriteReg(IMU3000_ADDR, Z_OFFS_USRL,(calib.z & 0x00FF));

    uint8_t buff[6];

    BurstRead(IMU3000_ADDR, X_OFFS_USRH, 6, buff);

    calib.x = buff[0]<<8 | buff[1];
    calib.y = buff[2]<<8 | buff[3];
    calib.z = buff[4]<<8 | buff[5];

#else
    calib.x = 28;
    calib.y = 9;
    calib.z = 18;
#endif

}

void IMU3000::WriteReg(uint8_t address, uint8_t reg, uint8_t value) {
    IMUWire->beginTransmission(address);
    IMUWire->send(reg);
    IMUWire->send(value);
    IMUWire->endTransmission();
}

uint8_t IMU3000::ReadReg(uint8_t address, uint8_t reg) {

    IMUWire->beginTransmission(address);
    IMUWire->send(reg);
    IMUWire->endTransmission();
    IMUWire->requestFrom(address, 1);
    return IMUWire->receive();
}

void IMU3000::BurstWrite(uint8_t address, uint8_t first_reg, uint8_t nbytes, uint8_t *buff) {
    uint8_t i;

    IMUWire->beginTransmission(address);
    IMUWire->send(first_reg);

    for(i = 0; i < nbytes ;i++)
        IMUWire->send(buff[i]);

    IMUWire->endTransmission();
}

void IMU3000::BurstRead(uint8_t address, uint8_t first_reg, uint8_t nbytes, uint8_t *buff) {
    uint8_t i;

    IMUWire->beginTransmission(address);
    IMUWire->send(first_reg);
    IMUWire->endTransmission();
    IMUWire->requestFrom(address, nbytes);

    for(i = 0; i < nbytes ;i++)
        buff[i] = IMUWire->receive();
}