File content as of revision 8:c6345e8d607c:

#include "Sensors.h"

I2C i2c(p29, p28);

const int AS5600 = 0x36 << 1;
const int MPU6050 = 0x68 << 1;

Sensors::Sensors()
{
    i2c.frequency(350000);
    
    for(int i = 0; i < 12; i++){
        imuData[i] = 0;
        }
        
    angle[0] = angle[1] = 0;
    angleDummy[0] = angleDummy[1] = 0;
    
    //setupAngle();
    setupIMU(0xfe);
}


int8_t Sensors::getAngleDummy(int n){
    angleDummy[n]++;
    return angleDummy[n];
    
    }

void Sensors::setupAngle()
{
    cmd[0] = 0x0C;
    i2c.write(AS5600,cmd,1);
    i2c.read(AS5600,out,2);
    
    cmd[0]= 0x01;
    cmd[1] = out[0];
    cmd[2] = out[1];
    i2c.write(AS5600,cmd,3);
    
}

void Sensors::wakeIMU(){
    writeRegister(0x6B, (readRegister(0x6B) & 0xbf));
}

void Sensors::disableIMU(){
    writeRegister(0x6B, (readRegister(0x6B) | (1 << 6)));
}


void Sensors::setupIMU(int8_t sampleFrequencyIMU)
{   

    
    //Set sample rate to 8000/1+79 = 100Hz
    writeRegister(0x19,sampleFrequencyIMU); //4f = 100 hz, 9f = 50 hz

    //Disable Frame Synchronization (FSYNC)
    writeRegister(0x1A,0x0);

    //Gyroscope Configuration
    //bit 4 and 3 is the full scale
    //0 = 250, 1 = 500, 2 = 1000, 3 = 2000
    writeRegister(0x1B,(0x0 << 3));

    //Accelerometer Configuration
    //bit 4 and 3 is the full scale
    //0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g
    writeRegister(0x1C,(0x3 << 3));

    //Set Motion Free Fall Threshold
    writeRegister(0x1D,0x0a);

    //Set Motion Free Fall Dur
    writeRegister(0x1E,0xff);

    //Set Motion Detection Threshold
    writeRegister(0x1F,0xf);

    //Set Motion Detection Dur
    writeRegister(0x20,0x0a);

    //Set Zero Motion Threshold
    writeRegister(0x21,0xf1);

    //Set Zero Motion Dur
    writeRegister(0x22,0x0a);

    //Disable FIFO buffer
    writeRegister(0x23,0x00);

    //Set aux I2C, from 0x24 to 0x36 = 0x00

    //Setup INT pin and AUX I2C pass through
    writeRegister(0x37,0x02);

    //Interrupt Enable
    //[7] FF_EN [6] MOT_EN  [5] ZMOT_EN [4] FIFO_OFLOW_EN   [3] I2C_MST_INT_EN  [2] PLL_RDY_INT_EN  [1] DMP_INT_EN  [0] RAW_RDY_EN
    //writeRegister(0x38,0xe1); 
    //writeRegister(0x38,0x1); 
    writeRegister(0x38, 0xe1);

    //Motion detection control
    writeRegister(0x69,(1<<4));

    //User control
    //[7] DMP_EN    [6] FIFO_EN [5] I2C_MST_EN  [4] I2C_IF_DIS  [3] DMP_RESET   [2] FIFO_RESET  [1] I2C_MST_RESET   [0] SIG_COND_RESET
    writeRegister(0x6A,0x80);

    //Set power managenemt 1
    //[7] DEVICE_RESET    [6] SLEEP   [5] CYCLE       [3] TEMP_DIS    [2:0] CLK_SEL
    writeRegister(0x6B,0x02);

    //Set power managenemt 2
    writeRegister(0x6C,0x00);
}

void Sensors::updateAngle()
{
    /* Read angle from AS5600 */
    cmd[0] = 0x0E;
    i2c.write(AS5600,cmd,1);
    i2c.read(AS5600,out,2);
    
    
    
    angle[0] = out[0];
    angle[1] = out[1];
}

void Sensors::updateIMU()
{
    cmd[0] = 0x3B;
    i2c.write(MPU6050,cmd,1,true);
    i2c.read(MPU6050,out,14);
    
    imuData[0] = out[0];
    imuData[1] = out[1];
    imuData[2] = out[2];
    imuData[3] = out[3];
    imuData[4] = out[4];
    imuData[5] = out[5];
    
    imuData[6] = out[8];
    imuData[7] = out[9];
    imuData[8] = out[10];
    imuData[9] = out[11];
    imuData[10] = out[12];
    imuData[11] = out[13];
}

int8_t Sensors::getInterruptStatus(){
    //[7] FF_INT  [6] MOT_INT [5] ZMOT_INT    [4] FIFO_OFLOW_INT  [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_IN
    return readRegister(0x3A);
}

int8_t Sensors::getMotionDetectionStatus(){
    //[7] MOT_XNEG    [6] MOT_XPOS    [5] MOT_YNEG    [4] MOT_YPOS    [3] MOT_ZNEG    [2] MOT_ZPOS
    return readRegister(0x61);
    }

uint8_t Sensors::getAngle(int i){
    return angle[i];
    }

int8_t Sensors::getIMU(int i){
    return imuData[i];
    }


int8_t Sensors::readRegister(int8_t addr)
{
    cmd[0] = addr;
    i2c.write(MPU6050,cmd,1);
    i2c.read(MPU6050,out,1);
    return out[0];
}

void Sensors::writeRegister(int8_t addr, int8_t value)
{
    cmd[0] = addr;
    cmd[1] = value;
    i2c.write(MPU6050,cmd,2);
}

//http://www.i2cdevlib.com/devices/mpu6050#registers