File content as of revision 0:c3e3dc7923c8:

#include "mbed.h"
#include "math.h"

#define BMP280_SLAVE_ADDRESS (0x76 << 1)
#define AK8963_ADDRESS 0x0C << 1
#define WHO_AM_I_AK8963 0x00
#define INFO 0x01
#define AK8963_ST1 0x02
#define AK8963_XOUT_L 0x03
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09
#define AK8963_CNTL 0x0A
#define AK8963_ASTC 0x0C
#define AK8963_I2CDIS 0x0F
#define AK8963_ASAX 0x10
#define AK8963_ASAY 0x11
#define AK8963_ASAZ 0x12
#define XG_OFFSET_H 0x13
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG2 0x1D
#define LP_ACCEL_ODR 0x1E
#define WOM_THR 0x1F
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define MOT_DETECT_STATUS 0x61
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A
#define PWR_MGMT_1 0x6B
#define PWR_MGMT_2 0x6C
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU9250 0x75
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E
#define P0 1015.0
#define PI 3.14159265358979323846f
#define ADO 0
#define MPU9250_ADDRESS 0x68 << 1 
#define I2C_SDA PB_7
#define I2C_SCL PB_6
I2C i2c(I2C_SDA, I2C_SCL);

enum Ascale
{
    AFS_2G = 0,
    AFS_4G,
    AFS_8G,
    AFS_16G
};

enum Gscale
{
    GFS_250DPS = 0,
    GFS_500DPS,
    GFS_1000DPS,
    GFS_2000DPS
};

enum Mscale
{
    MFS_14BITS = 0,
    MFS_16BITS     
};
uint8_t Ascale;    
uint8_t Gscale;    
uint8_t Mscale;    
uint8_t Mmode;     
float aRes, gRes, mRes;

int16_t accelCount[3]; 
int16_t gyroCount[3];  
int16_t magCount[3];
float q[4];         

float ax, ay, az, gx, gy, gz, mx, my, mz;
float pitch, yaw, roll;
float deltat;                     
int lastUpdate, firstUpdate, Now; 
int delt_t;                       
int count;                        
int16_t tempCount;                
float temperature;

class LOGGER
{
private:
    char address;
    uint16_t dig_T1;
    int16_t dig_T2, dig_T3;
    uint16_t dig_P1;
    int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
    int32_t t_fine;

public:
    float pressf;
    float tempf;

    LOGGER(char slave_adr = BMP280_SLAVE_ADDRESS) : address(slave_adr), t_fine(0)
    {
        initialize();
    }

    void initialize()
    {
        char cmd[18];
        cmd[0] = 0xf2;
        cmd[1] = 0x01;
        i2c.write(address, cmd, 2);
        cmd[0] = 0xf4;
        cmd[1] = 0x27;
        i2c.write(address, cmd, 2);
        cmd[0] = 0xf5; 
        cmd[1] = 0xa0; 
        i2c.write(address, cmd, 2);
        cmd[0] = 0x88; 
        i2c.write(address, cmd, 1);
        i2c.read(address, cmd, 6);
        dig_T1 = (cmd[1] << 8) | cmd[0];
        dig_T2 = (cmd[3] << 8) | cmd[2];
        dig_T3 = (cmd[5] << 8) | cmd[4];
        cmd[0] = 0x8E; 
        i2c.write(address, cmd, 1);
        i2c.read(address, cmd, 18);
        dig_P1 = (cmd[1] << 8) | cmd[0];
        dig_P2 = (cmd[3] << 8) | cmd[2];
        dig_P3 = (cmd[5] << 8) | cmd[4];
        dig_P4 = (cmd[7] << 8) | cmd[6];
        dig_P5 = (cmd[9] << 8) | cmd[8];
        dig_P6 = (cmd[11] << 8) | cmd[10];
        dig_P7 = (cmd[13] << 8) | cmd[12];
        dig_P8 = (cmd[15] << 8) | cmd[14];
        dig_P9 = (cmd[17] << 8) | cmd[16];
    }
    float getTemperature()
    {
        uint32_t temp_raw;
        float tempf;
        char cmd[4];
        cmd[0] = 0xfa;
        i2c.write(address, cmd, 1);
        i2c.read(address, &cmd[1], 3);
        temp_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
        int32_t temp;
        temp =
            (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
            ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
        t_fine = temp;
        temp = (temp * 5 + 128) >> 8;
        tempf = (float)temp;
        return (tempf / 100.0f);
    }
    float getPressure()
    {
        uint32_t press_raw;
        float pressf;
        char cmd[4];
        cmd[0] = 0xf7; // press_msb
        i2c.write(address, cmd, 1);
        i2c.read(address, &cmd[1], 3);
        press_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);

        int32_t var1, var2;
        uint32_t press;

        var1 = (t_fine >> 1) - 64000;
        var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
        var2 = var2 + ((var1 * dig_P5) << 1);
        var2 = (var2 >> 2) + (dig_P4 << 16);
        var1 = (((dig_P3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
        var1 = ((32768 + var1) * dig_P1) >> 15;
        if (var1 == 0)
        {
            return 0;
        }
        press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
        if (press < 0x80000000)
        {
            press = (press << 1) / var1;
        }
        else
        {
            press = (press / var1) * 2;
        }
        var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
        var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
        press = (press + ((var1 + var2 + dig_P7) >> 4));
        pressf = (float)press;
        return (pressf / 100.0f);
    }
    void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
    {
        char data_write[2];
        data_write[0] = subAddress;
        data_write[1] = data;
        i2c.write(address, data_write, 2, 0);
    }

    char readByte(uint8_t address, uint8_t subAddress)
    {
        char data[1];
        char data_write[1];
        data_write[0] = subAddress;
        i2c.write(address, data_write, 1, 1);
        i2c.read(address, data, 1, 0);
        return data[0];
    }

    void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
    {
        char data[14];
        char data_write[1];
        data_write[0] = subAddress;
        i2c.write(address, data_write, 1, 1);
        i2c.read(address, data, count, 0);
        for (int ii = 0; ii < count; ii++)
        {
            dest[ii] = data[ii];
        }
    }

    void getMres()
    {
        switch (Mscale)
        {
        case MFS_14BITS:
            mRes = 10.0 * 4219.0 / 8190.0;
            break;
        case MFS_16BITS:
            mRes = 10.0 * 4219.0 / 32760.0;
            break;
        }
    }

    void getGres()
    {
        switch (Gscale)
        {
        case GFS_250DPS:
            gRes = 250.0 / 32768.0;
            break;
        case GFS_500DPS:
            gRes = 500.0 / 32768.0;
            break;
        case GFS_1000DPS:
            gRes = 1000.0 / 32768.0;
            break;
        case GFS_2000DPS:
            gRes = 2000.0 / 32768.0;
            break;
        }
    }

    void getAres()
    {
        switch (Ascale)
        {
        case AFS_2G:
            aRes = 2.0 / 32768.0;
            break;
        case AFS_4G:
            aRes = 4.0 / 32768.0;
            break;
        case AFS_8G:
            aRes = 8.0 / 32768.0;
            break;
        case AFS_16G:
            aRes = 16.0 / 32768.0;
            break;
        }
    }

    void readAccelData(int16_t *destination)
    {
        uint8_t rawData[6];                                                  
        readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);            
        destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); 
        destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
        destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
    }

    void readGyroData(int16_t *destination)
    {
        uint8_t rawData[6];                                                  
        readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);             
        destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); 
        destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
        destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
    }

    void readMagData(int16_t *destination)
    {
        uint8_t rawData[7]; 
        if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01)
        {                                               
            readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
            uint8_t c = rawData[6];                                  
            if (!(c & 0x08))
            {                                                        
                destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);
                destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]);
                destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]);
            }
        }
    }

    int16_t readTempData()
    {
        uint8_t rawData[2];                                     
        readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); 
        return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]); 
    }

    void resetMPU9250()
    {
        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
        wait(0.1);
    }

    void initAK8963(float *destination)
    {
        uint8_t rawData[3];                    
        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
        wait(0.01);
        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F);
        wait(0.01);
        readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);
        destination[0] = (float)(rawData[0] - 128) / 256.0f + 1.0f;
        destination[1] = (float)(rawData[1] - 128) / 256.0f + 1.0f;
        destination[2] = (float)(rawData[2] - 128) / 256.0f + 1.0f;
        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
        wait(0.01);
        writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode);
        wait(0.01);
    }

    void initMPU9250()
    {
        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
        wait(0.1); 
        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
        writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
        writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);
        uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0);
        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18);
        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3);

        c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0);
        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18);
        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3);
        c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F);
        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03);
        writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
        writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);
    }
//    void calibrateMPU9250(float *dest1, float *dest2)
//    {
//        uint8_t data[12];
//        uint16_t ii, packet_count, fifo_count;
//        int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
//
//        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); 
//        wait(0.1);
//        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
//        writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
//        wait(0.2);
//        writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);  
//        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);     
//        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);  
//        writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00);
//        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); 
//        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); 
//        wait(0.015);
//        writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      
//        writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  
//        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); 
//        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
//
//        uint16_t gyrosensitivity = 131;    
//        uint16_t accelsensitivity = 16384; 
//        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); 
//        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);   
//        wait(0.04);
//        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);           
//        readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]);
//        fifo_count = ((uint16_t)data[0] << 8) | data[1];
//        packet_count = fifo_count / 12;
//
//        for (ii = 0; ii < packet_count; ii++)
//        {
//            int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
//            readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]);
//            accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]);
//            accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
//            accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
//            gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
//            gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
//            gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
//
//            accel_bias[0] += (int32_t)accel_temp[0];
//            accel_bias[1] += (int32_t)accel_temp[1];
//            accel_bias[2] += (int32_t)accel_temp[2];
//            gyro_bias[0] += (int32_t)gyro_temp[0];
//            gyro_bias[1] += (int32_t)gyro_temp[1];
//            gyro_bias[2] += (int32_t)gyro_temp[2];
//        }
//        accel_bias[0] /= (int32_t)packet_count;
//        accel_bias[1] /= (int32_t)packet_count;
//        accel_bias[2] /= (int32_t)packet_count;
//        gyro_bias[0] /= (int32_t)packet_count;
//        gyro_bias[1] /= (int32_t)packet_count;
//        gyro_bias[2] /= (int32_t)packet_count;
//
//        if (accel_bias[2] > 0L)
//        {
//            accel_bias[2] -= (int32_t)accelsensitivity;
//        }
//        else
//        {
//            accel_bias[2] += (int32_t)accelsensitivity;
//        }
//        data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF;
//        data[1] = (-gyro_bias[0] / 4) & 0xFF;
//        data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
//        data[3] = (-gyro_bias[1] / 4) & 0xFF;
//        data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
//        data[5] = (-gyro_bias[2] / 4) & 0xFF;
//
//        dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
//        dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
//        dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity;
//
//        int32_t accel_bias_reg[3] = {0, 0, 0};               
//        readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]);
//        accel_bias_reg[0] = (int16_t)((int16_t)data[0] << 8) | data[1];
//        readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
//        accel_bias_reg[1] = (int16_t)((int16_t)data[0] << 8) | data[1];
//        readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
//        accel_bias_reg[2] = (int16_t)((int16_t)data[0] << 8) | data[1];
//
//        uint32_t mask = 1uL;            
//        uint8_t mask_bit[3] = {0, 0, 0};
//
//        for (ii = 0; ii < 3; ii++)
//        {
//            if (accel_bias_reg[ii] & mask)
//                mask_bit[ii] = 0x01; 
//        }
//        accel_bias_reg[0] -= (accel_bias[0] / 8);
//        accel_bias_reg[1] -= (accel_bias[1] / 8);
//        accel_bias_reg[2] -= (accel_bias[2] / 8);
//        data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
//        data[1] = (accel_bias_reg[0]) & 0xFF;
//        data[1] = data[1] | mask_bit[0];
//        data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
//        data[3] = (accel_bias_reg[1]) & 0xFF;
//        data[3] = data[3] | mask_bit[1];
//        data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
//        data[5] = (accel_bias_reg[2]) & 0xFF;
//        data[5] = data[5] | mask_bit[2];
//        dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
//        dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;
//        dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;
//    }
};