File content as of revision 1:b44bd62c542f:

#include "Sensors.h"

I2C i2c(p29, p28);

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

Sensors::Sensors()
{
    i2c.frequency(300000);
    this->AS5600 = 0x36 << 1;
    this->MPU6500 = 0x68 << 1;

    this->angle = 0;
    this->accel_x = 0;
    this->accel_y = 0;
    this->accel_z = 0;
    this->temp = 0;
    this->gyro_x = 0;
    this->gyro_y = 0;
    this->gyro_z = 0;

    setupIMU();
    setupAngle();
}


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

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

    angle = ((out[0] << 8) + out[1]) * 0.087912087 ;
    return angle;
}

void Sensors::setupAngle()
{
}

void Sensors::setupIMU()
{
    /* Setup MPU6500 */
    // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
    cmd[0] = 0x6B;
    i2c.write(MPU6500,cmd,1);
    i2c.read(MPU6500,out,1);
    int PW = out[0];

    // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
    cmd[1] = (PW & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
    PW = cmd[1];
    i2c.write(MPU6500,cmd,2);

    // Set resulution for gyroscope and accelerometer
    cmd[0] = 0x1B;
    cmd[1] = 0x00;
    cmd[2] = 0x00;
    i2c.write(MPU6500,cmd,3);

    // Set Reset = 0 & Sleep = 0
    cmd[0] = 0x6B;
    cmd[1] = PW & 0x3F;
    i2c.write(MPU6500,cmd,2);
}

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

int16_t Sensors::getAccelX()
{
    return this->accel_x;
}
int16_t Sensors::getAccelY()
{
    return this->accel_y;
}
int16_t Sensors::getAccelZ()
{
    return this->accel_z;
}
float Sensors::getTemp()
{
    return (this->temp)/340 + 36.53;
}
int16_t Sensors::getGyroX()
{
    return this->accel_x;
}
int16_t Sensors::getGyroY()
{
    return this->gyro_y;
}
int16_t Sensors::getGyroZ()
{
    return this->gyro_z;
}

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);
}

void Sensors::setDMP(int8_t th)
{
    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 3))); //reset DMP
    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 7))); //set DMP
    writeRegister(0x38, ( readRegister(0x38) | (1 << 6))); //enable motion interrupt
    writeRegister(0x38, ( readRegister(0x38) | (1 << 1))); //enable motion interrupt
    writeRegister(0x1F, th); //set threshold
}


bool Sensors::checkDMP()
{
    
    if (((readRegister(0x3A) >> 6) & 1) == 1) {
        return true;
    }  else {
        return false;
    }
}