File content as of revision 4:2a5a08b14539:

#include "Sensors.h"

I2C i2c(p29, p28);

Sensors* Sensors::instance = new Sensors();

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

Sensors::Sensors()
{
    i2c.frequency(300000);

    this->angle_h = 0;
    this->angle_l = 0;
    
    this->accel_x_h = 0;
    this->accel_y_h = 0;
    this->accel_z_h = 0;
    this->accel_x_l = 0;
    this->accel_y_l = 0;
    this->accel_z_l = 0;
    
    this->gyro_x_h = 0;
    this->gyro_y_h = 0;
    this->gyro_z_h = 0;
    this->gyro_x_l = 0;
    this->gyro_y_l = 0;
    this->gyro_z_l = 0;


    setupIMU();
    setupAngle();
}


Sensors* Sensors::getInstance()
{
    return instance;
}

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

    this->angle_h = out[0];   //((out[0] << 8) + out[1]) * 0.087912087;
    this->angle_l = out[1];
}

float Sensors::getAngle(){
    return ((getAngleH() << 8) + getAngleL()) * 0.087912087;
    }

char Sensors::getAngleH(){
    return this->angle_h;
}
char Sensors::getAngleL(){
    return this->angle_l;
}

void Sensors::setupAngle()
{
    //TODO
}

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

void Sensors::setupIMU()
{
    //Sets sample rate to 8000/1+79 = 100Hz
    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
    
    wait(0.001);
    
    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
    //Disable FSync, 256 DLPF
    writeRegister(0x1A,0x00);
    
    // Disable gyroscope self tests, scale of 200 degrees/s
    // Disable accelerometer self tests, scale of +-2g, no DHPF
    cmd[0] = 0x1B;
    cmd[1] = 0x00;  //bit4 and bit 3 to choose scale range
    cmd[2] = 0x00;
    i2c.write(MPU6500,cmd,3);
    
    //Freefall threshold of 0mg and duration limit of 0
    cmd[0] = 0x1D;
    i2c.write(MPU6500,cmd,3);
    
    //Motion threshold of 0mg and duration limit of 0
    cmd[0] = 0x21;
    i2c.write(MPU6500,cmd,3);
    
    //Disable sensor output FIFO buffer
    writeRegister(0x23,0x00);
    
    //Set aux I2C, from 0x24 to 0x36 = 0x00 
    
    //Setup INT pin and AUX I2C pass through
    cmd[0] = 0x37;
    cmd[1] = 0x02;
    cmd[2] = 0x01; 
    i2c.write(MPU6500,cmd,3);
    
    //Reset sensor signal paths
    writeRegister(0x68,0x00);
    
    //Motion detection control
    writeRegister(0x69,0x00);
    
    //Disables FIFO, AUX I2C and I2C reset bits to 0
    writeRegister(0x6A,0x00);
    
    //Sets clock source to gyro reference w/ PLL
    /* SLEEP = 0 */
    writeRegister(0x6B,0x02);
    
    //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
    writeRegister(0x6C,0x00);

    //Data transfer to and from the FIFO buffer
    writeRegister(0x74,0x00);
}

void Sensors::updateIMU()
{
    // Read measurements from MPU6500  
    cmd[0] = 0x3B;
    i2c.write(MPU6500,cmd,1,true);
    i2c.read(MPU6500,out,14);
    /*
    this->accel_x = (out[0] << 8) | out[1];
    this->accel_y = (out[2] << 8) | out[3];
    this->accel_z = (out[4] << 8) | out[5];

    this->temp = (out[6] << 8) | out[7];

    this->gyro_x = (out[8] << 8) | out[9];
    this->gyro_y = (out[10] << 8) | out[11];
    this->gyro_z = (out[12] << 8) | out[13];
    
    */
    
    this->accel_x_h = out[0];
    this->accel_x_l = out[1];
    this->accel_y_h = out[2];
    this->accel_y_l = out[3];
    this->accel_z_h = out[4];
    this->accel_z_l = out[5];


    this->gyro_x_h = out[8];
    this->gyro_x_l = out[9];
    this->gyro_y_h = out[10];
    this->gyro_y_l = out[11];
    this->gyro_z_h = out[12];
    this->gyro_z_l = out[13];
    
}

int8_t Sensors::getAccelXH(){
    return this->accel_x_h;
}
int8_t Sensors::getAccelXL(){
    return this->accel_x_l;
}
int8_t Sensors::getAccelYH(){
    return this->accel_y_h;
}
int8_t Sensors::getAccelYL(){
    return this->accel_y_l;
}
int8_t Sensors::getAccelZH(){
    return this->accel_z_h;
}
int8_t Sensors::getAccelZL(){
    return this->accel_z_l;
}


int8_t Sensors::getGyroXH(){
    return this->gyro_x_h;
}
int8_t Sensors::getGyroXL(){
    return this->gyro_x_l;
}
int8_t Sensors::getGyroYH(){
    return this->gyro_y_h;
}
int8_t Sensors::getGyroYL(){
    return this->gyro_y_l;
}
int8_t Sensors::getGyroZH(){
    return this->gyro_z_h;
}
int8_t Sensors::getGyroZL(){
    return this->gyro_z_l;
}


int16_t Sensors::getAccelX(){
    return ((getAccelXH() << 8) + getAccelXL());
    }
int16_t Sensors::getAccelY(){
    return ((getAccelYH() << 8) + getAccelYL());
    }
int16_t Sensors::getAccelZ(){
    return ((getAccelZH() << 8) + getAccelZL());
    }
    
int16_t Sensors::getGyroX(){
    return ((getGyroXH() << 8) + getGyroXL());
    }


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

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