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@59:b709711bc566, 2017-04-08 (annotated)

Committer:
hazheng
Date:
Sat Apr 08 17:40:13 2017 +0000
Revision:
59:b709711bc566
Child:
62:bc5caf59fe39
Added IMUManager Class.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 59:b709711bc566 1 #include "IMUManager.h"
hazheng 59:b709711bc566 2 #include "PinAssignment.h"
hazheng 59:b709711bc566 3
hazheng 59:b709711bc566 4 #ifdef __cplusplus
hazheng 59:b709711bc566 5 extern "C" {
hazheng 59:b709711bc566 6 #endif
hazheng 59:b709711bc566 7
hazheng 59:b709711bc566 8 static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
hazheng 59:b709711bc566 9 static const int SLAVE_ADDR_READ = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;
hazheng 59:b709711bc566 10
hazheng 59:b709711bc566 11 static volatile struct imu_vec3 accel_value;
hazheng 59:b709711bc566 12 static volatile struct imu_vec3 magt_value;
hazheng 59:b709711bc566 13 static I2C imu_i2c_port(PIN_IMC_SDA, PIN_IMC_SCL);
hazheng 59:b709711bc566 14 static DigitalOut imu_accl_sa0(PIN_IMC_ACCL_SA0, ACCEL_MAG_SA0);
hazheng 59:b709711bc566 15 static DigitalOut imu_accl_sa1(PIN_IMC_ACCL_SA1, ACCEL_MAG_SA1);
hazheng 59:b709711bc566 16 static char imu_data_buffer[FXOS8700CQ_READ_LEN];
hazheng 59:b709711bc566 17
hazheng 59:b709711bc566 18 inline void imu_i2c_write_8bit(const char addr, const char* buffer)
hazheng 59:b709711bc566 19 {
hazheng 59:b709711bc566 20 imu_i2c_port.lock();
hazheng 59:b709711bc566 21 imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 59:b709711bc566 22 imu_i2c_port.write(SLAVE_ADDR_WRITE, buffer, 1, false);
hazheng 59:b709711bc566 23 imu_i2c_port.unlock();
hazheng 59:b709711bc566 24 }
hazheng 59:b709711bc566 25
hazheng 59:b709711bc566 26 inline void imu_i2c_read_8bit(const char addr, char* buffer)
hazheng 59:b709711bc566 27 {
hazheng 59:b709711bc566 28 imu_i2c_port.lock();
hazheng 59:b709711bc566 29 imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 59:b709711bc566 30 imu_i2c_port.read( SLAVE_ADDR_READ, buffer, 1, false);
hazheng 59:b709711bc566 31 imu_i2c_port.unlock();
hazheng 59:b709711bc566 32 }
hazheng 59:b709711bc566 33
hazheng 59:b709711bc566 34 inline void imu_i2c_read(const char addr, char* buffer, const int length)
hazheng 59:b709711bc566 35 {
hazheng 59:b709711bc566 36 imu_i2c_port.lock();
hazheng 59:b709711bc566 37 imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 59:b709711bc566 38 imu_i2c_port.read( SLAVE_ADDR_READ, buffer, length, false);
hazheng 59:b709711bc566 39 imu_i2c_port.unlock();
hazheng 59:b709711bc566 40 }
hazheng 59:b709711bc566 41
hazheng 59:b709711bc566 42 uint8_t imu_manager_init()
hazheng 59:b709711bc566 43 {
hazheng 59:b709711bc566 44 char dataBuf = 0;
hazheng 59:b709711bc566 45 imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
hazheng 59:b709711bc566 46 if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
hazheng 59:b709711bc566 47 {
hazheng 59:b709711bc566 48 return 0;
hazheng 59:b709711bc566 49 }
hazheng 59:b709711bc566 50
hazheng 59:b709711bc566 51 dataBuf = 0x00;
hazheng 59:b709711bc566 52 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 53 dataBuf = 0x1F;
hazheng 59:b709711bc566 54 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 55 dataBuf = 0x20;
hazheng 59:b709711bc566 56 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG2, &dataBuf);
hazheng 59:b709711bc566 57 dataBuf = 0x01;
hazheng 59:b709711bc566 58 imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
hazheng 59:b709711bc566 59 dataBuf = 0x0D;
hazheng 59:b709711bc566 60 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 61
hazheng 59:b709711bc566 62 return 1;
hazheng 59:b709711bc566 63 }
hazheng 59:b709711bc566 64
hazheng 59:b709711bc566 65 void imu_manager_update()
hazheng 59:b709711bc566 66 {
hazheng 59:b709711bc566 67 imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
hazheng 59:b709711bc566 68 // copy the 14 bit accelerometer byte data into 16 bit words
hazheng 59:b709711bc566 69 accel_value.x = (((static_cast<uint16_t>(imu_data_buffer[1]) << 8) | imu_data_buffer[2]))>> 2;
hazheng 59:b709711bc566 70 accel_value.y = (((static_cast<uint16_t>(imu_data_buffer[3]) << 8) | imu_data_buffer[4]))>> 2;
hazheng 59:b709711bc566 71 accel_value.z = (((static_cast<uint16_t>(imu_data_buffer[5]) << 8) | imu_data_buffer[6]))>> 2;
hazheng 59:b709711bc566 72 // copy the magnetometer byte data into 16 bit words
hazheng 59:b709711bc566 73 magt_value.x = (static_cast<uint16_t>(imu_data_buffer[7]) << 8) | imu_data_buffer[8];
hazheng 59:b709711bc566 74 magt_value.y = (static_cast<uint16_t>(imu_data_buffer[9]) << 8) | imu_data_buffer[10];
hazheng 59:b709711bc566 75 magt_value.z = (static_cast<uint16_t>(imu_data_buffer[11]) << 8) | imu_data_buffer[12];
hazheng 59:b709711bc566 76 }
hazheng 59:b709711bc566 77
hazheng 59:b709711bc566 78 const volatile struct imu_vec3* imu_manager_get_accl()
hazheng 59:b709711bc566 79 {
hazheng 59:b709711bc566 80 return &accel_value;
hazheng 59:b709711bc566 81 }
hazheng 59:b709711bc566 82
hazheng 59:b709711bc566 83 const volatile struct imu_vec3* imu_manager_get_magt()
hazheng 59:b709711bc566 84 {
hazheng 59:b709711bc566 85 return &magt_value;
hazheng 59:b709711bc566 86 }
hazheng 59:b709711bc566 87
hazheng 59:b709711bc566 88 #ifdef __cplusplus
hazheng 59:b709711bc566 89 }
hazheng 59:b709711bc566 90 #endif