Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/IMUManager.cpp

Committer:
hazheng
Date:
2017-04-08
Revision:
59:b709711bc566
Child:
62:bc5caf59fe39

File content as of revision 59:b709711bc566:

#include "IMUManager.h"
#include "PinAssignment.h"

#ifdef __cplusplus
extern "C" {
#endif

static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
static const int SLAVE_ADDR_READ  = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;

static volatile struct imu_vec3 accel_value;
static volatile struct imu_vec3 magt_value;
static I2C imu_i2c_port(PIN_IMC_SDA, PIN_IMC_SCL);
static DigitalOut imu_accl_sa0(PIN_IMC_ACCL_SA0, ACCEL_MAG_SA0);
static DigitalOut imu_accl_sa1(PIN_IMC_ACCL_SA1, ACCEL_MAG_SA1);
static char imu_data_buffer[FXOS8700CQ_READ_LEN];

inline void imu_i2c_write_8bit(const char addr, const char* buffer)
{
    imu_i2c_port.lock();
    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
    imu_i2c_port.write(SLAVE_ADDR_WRITE, buffer, 1, false);
    imu_i2c_port.unlock();
}

inline void imu_i2c_read_8bit(const char addr, char* buffer)
{
    imu_i2c_port.lock();
    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
    imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, 1, false);
    imu_i2c_port.unlock();
}

inline void imu_i2c_read(const char addr, char* buffer, const int length)
{
    imu_i2c_port.lock();
    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
    imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, length, false);
    imu_i2c_port.unlock();
}

uint8_t imu_manager_init()
{
    char dataBuf = 0;
    imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
    if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
    {
        return 0;   
    }
    
    dataBuf = 0x00;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    dataBuf = 0x1F;
    imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
    dataBuf = 0x20;
    imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG2, &dataBuf);
    dataBuf = 0x01;
    imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
    dataBuf = 0x0D;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    
    return 1;
}

void imu_manager_update()
{
    imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
    // copy the 14 bit accelerometer byte data into 16 bit words
    accel_value.x = (((static_cast<uint16_t>(imu_data_buffer[1]) << 8) | imu_data_buffer[2]))>> 2;
    accel_value.y = (((static_cast<uint16_t>(imu_data_buffer[3]) << 8) | imu_data_buffer[4]))>> 2;
    accel_value.z = (((static_cast<uint16_t>(imu_data_buffer[5]) << 8) | imu_data_buffer[6]))>> 2;
    // copy the magnetometer byte data into 16 bit words
    magt_value.x = (static_cast<uint16_t>(imu_data_buffer[7]) << 8) | imu_data_buffer[8];
    magt_value.y = (static_cast<uint16_t>(imu_data_buffer[9]) << 8) | imu_data_buffer[10];
    magt_value.z = (static_cast<uint16_t>(imu_data_buffer[11]) << 8) | imu_data_buffer[12];
}

const volatile struct imu_vec3* imu_manager_get_accl()
{
    return &accel_value;
}

const volatile struct imu_vec3* imu_manager_get_magt()
{
    return &magt_value;
}

#ifdef __cplusplus
}
#endif