imu.c

#define _SUPPRESS_PLIB_WARNING
#define _DISABLE_OPENADC10_CONFIGPORT_WARNING
#include <plib.h>
#include "imu.h"
#include <math.h>
#include <limits.h>
#include "servo.h"
#include "pt_cornell_1_2_2.h"

#define MPU_ADDRESS 0xd0
#define AK8963_ADDRESS (0x0C<<1)

#define MPU_INT_PIN_CFG 0x37
#define MPU_FIFO_EN 0x23
#define MPU_I2C_MST_CTRL 0x24
#define MPU_USER_CTRL 0x6a
#define MPU_INT_ENABLE 0x38
#define MPU_PWR_MGMT_1 0x6B

#define AK8963_WIA 0x00
#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_CNTL1 0x0A
#define AK8963_ASTC 0x0C
#define AK8963_ASAX 0x10
#define AK8963_ASAY 0x11
#define AK8963_ASAZ 0x12


int xmax = 0;
int xmin = 0;
int ymax = 0;
int ymin = 0;
static char error;

// Wait by executing nops

static void i2c_wait(unsigned int cnt) {
    while (--cnt) {
        asm("nop");
        asm("nop");
    }
}

static void i2c_write_byte(char device, char address, char data) {
    char i2c_header[2];
    i2c_header[0] = device | 0; // device address & WR
    i2c_header[1] = address; // register address

    StartI2C2(); // Send the Start Bit
    IdleI2C2(); // Wait to complete

    int i;
    for (i = 0; i < 2; i++) {
        MasterWriteI2C2(i2c_header[i]);
        IdleI2C2(); // Wait to complete

        // ACKSTAT is 0 when slave acknowledge,
        // if 1 then slave has not acknowledge the data.
        if (I2C2STATbits.ACKSTAT) break;
    }
    MasterWriteI2C2(data);
    IdleI2C2(); // Wait to complete

    StopI2C2(); // Send the Stop condition
    IdleI2C2(); // Wait to complete
}

static char i2c_read_device(char device, char address) {

    char i2c_header[2];
    i2c_header[0] = (device | 0); // device address & WR
    i2c_header[1] = address; // register address

    StartI2C2(); // Send the Start Bit
    IdleI2C2(); // Wait to complete

    int i;
    for (i = 0; i < 2; i++) {
        MasterWriteI2C2(i2c_header[i]);
        IdleI2C2(); // Wait to complete

        // ACKSTAT is 0 when slave acknowledge,
        // if 1 then slave has not acknowledge the data.
        if (I2C2STATbits.ACKSTAT) {
            break;
        }
    }

    // now send a start sequence again
    RestartI2C2(); // Send the Restart condition
    i2c_wait(10);
    // wait for this bit to go back to zero
    IdleI2C2(); // Wait to complete

    MasterWriteI2C2(device | 1); // transmit read command
    IdleI2C2(); // Wait to complete

    char data = MasterReadI2C2();

    IdleI2C2(); // Wait to complete

    StopI2C2(); // Send the Stop condition
    IdleI2C2(); // Wait to complete


    //    i2c_wait(10000000);
    return data;
}

int imu_mag_read_data(int* destination) {
    i2c_write_byte(AK8963_ADDRESS, AK8963_CNTL1, 0x01);
    // single read 14-bit

    // i2c_write_byte(AK8963_ADDRESS, AK8963_CNTL1, 1 << 4 | 0x01);
    // single read 16-bit
    // i2c_write_byte(AK8963_ADDRESS, AK8963_CNTL1, 0x02); 
    // cont mode 8 kHz

    char newMagData = (i2c_read_device(AK8963_ADDRESS, AK8963_ST1) & 0x01);
    //    if (newMagData) { // wait for magnetometer data ready bit to be set
    char rawdata[7];
    int i;
    for (i = 0; i < 7; i++) { // End data read by reading ST2 register
        rawdata[i] = i2c_read_device(AK8963_ADDRESS, AK8963_XOUT_L + i);
    }

    i2c_wait(100000);

    if (!(rawdata[6] & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
        destination[0] = (rawdata[1] << 8) | rawdata[0]; // Turn the MSB and LSB into a signed 16-bit value
        destination[1] = (rawdata[3] << 8) | rawdata[2]; // Data stored as little Endian
        destination[2] = (rawdata[5] << 8) | rawdata[4];

        return 0;
    } else {
        return 1;
    }
}

int imu_get_heading() {

    int mag[3];
    float x, y;
    int i;
    //    for (i = 0; i < 3; i++) {
    error = imu_mag_read_data(mag);

    int xoffset = (xmax + xmin) / 2;
    int yoffset = (ymax + ymin) / 2;

    x = ((float) mag[0] - (float) xoffset) / (float) (xmax - xmin);
    y = ((float) mag[1] - (float) yoffset) / (float) (ymax - ymin);
    //    }
    return (int) (atan2(x / 3, y / 3) * (180 / 3.1416));
}

int imu_get_x_raw(int axis) {
    int mag[3];
    error = imu_mag_read_data(mag);

    return mag[axis];
}

int imu_last_error() {
    return error;
}

void imu_init() {
    mPORTBSetPinsDigitalOut(BIT_8 | BIT_9);
    OpenI2C2(I2C_ON, 48);

    i2c_write_byte(MPU_ADDRESS, MPU_INT_PIN_CFG, 0x22); //pass through enable
    i2c_write_byte(MPU_ADDRESS, MPU_INT_ENABLE, 0x01);
    i2c_write_byte(MPU_ADDRESS, MPU_I2C_MST_CTRL, 0x00); // Disable I2C master
    i2c_write_byte(MPU_ADDRESS, MPU_PWR_MGMT_1, 0x00); //power up sensors


}

int degree(int deg) {
    if (deg < -180) {
        return deg + 360;
    } else if (deg > 180) {
        return deg - 360;
    } else {
        return deg;
    }
}

int angle_diff(int source, int target) {
    int a = target - source;
    a += (a > 180) ? -360 : (a<-180) ? 360 : 0;
    return a;
}