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@62:bc5caf59fe39, 2017-04-09 (annotated)

Committer:
hazheng
Date:
Sun Apr 09 18:20:57 2017 +0000
Revision:
62:bc5caf59fe39
Parent:
59:b709711bc566
Child:
63:d9a81b3d69f5
Made the IMU working correctly.

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 62:bc5caf59fe39 4 //#define SW_DEBUG
hazheng 62:bc5caf59fe39 5 #include "SWCommon.h"
hazheng 62:bc5caf59fe39 6
hazheng 59:b709711bc566 7 #ifdef __cplusplus
hazheng 59:b709711bc566 8 extern "C" {
hazheng 59:b709711bc566 9 #endif
hazheng 59:b709711bc566 10
hazheng 59:b709711bc566 11 static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
hazheng 59:b709711bc566 12 static const int SLAVE_ADDR_READ = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;
hazheng 59:b709711bc566 13
hazheng 59:b709711bc566 14 static volatile struct imu_vec3 accel_value;
hazheng 59:b709711bc566 15 static volatile struct imu_vec3 magt_value;
hazheng 59:b709711bc566 16 static I2C imu_i2c_port(PIN_IMC_SDA, PIN_IMC_SCL);
hazheng 59:b709711bc566 17 static DigitalOut imu_accl_sa0(PIN_IMC_ACCL_SA0, ACCEL_MAG_SA0);
hazheng 59:b709711bc566 18 static DigitalOut imu_accl_sa1(PIN_IMC_ACCL_SA1, ACCEL_MAG_SA1);
hazheng 62:bc5caf59fe39 19 static char imu_data_buffer[FXOS8700CQ_READ_LEN] = { 0 };
hazheng 59:b709711bc566 20
hazheng 59:b709711bc566 21 inline void imu_i2c_write_8bit(const char addr, const char* buffer)
hazheng 59:b709711bc566 22 {
hazheng 62:bc5caf59fe39 23 static char write_buf[2];
hazheng 62:bc5caf59fe39 24 write_buf[0] = addr;
hazheng 62:bc5caf59fe39 25 write_buf[1] = *buffer;
hazheng 62:bc5caf59fe39 26 imu_i2c_port.write(SLAVE_ADDR_WRITE, write_buf, 2, false);
hazheng 59:b709711bc566 27 }
hazheng 59:b709711bc566 28
hazheng 59:b709711bc566 29 inline void imu_i2c_read_8bit(const char addr, char* buffer)
hazheng 59:b709711bc566 30 {
hazheng 62:bc5caf59fe39 31 int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 62:bc5caf59fe39 32 int t2 = imu_i2c_port.read( SLAVE_ADDR_READ, buffer, 1, false);
hazheng 59:b709711bc566 33 }
hazheng 59:b709711bc566 34
hazheng 59:b709711bc566 35 inline void imu_i2c_read(const char addr, char* buffer, const int length)
hazheng 59:b709711bc566 36 {
hazheng 62:bc5caf59fe39 37 int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 62:bc5caf59fe39 38 int t2 = imu_i2c_port.read( SLAVE_ADDR_READ, buffer, length, false);
hazheng 59:b709711bc566 39 }
hazheng 59:b709711bc566 40
hazheng 59:b709711bc566 41 uint8_t imu_manager_init()
hazheng 59:b709711bc566 42 {
hazheng 62:bc5caf59fe39 43 accel_value.x = accel_value.y = accel_value.z = 0;
hazheng 62:bc5caf59fe39 44 magt_value.x = magt_value.y = magt_value.z = 0;
hazheng 62:bc5caf59fe39 45
hazheng 59:b709711bc566 46 char dataBuf = 0;
hazheng 59:b709711bc566 47 imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
hazheng 59:b709711bc566 48 if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
hazheng 59:b709711bc566 49 {
hazheng 62:bc5caf59fe39 50 return dataBuf;
hazheng 59:b709711bc566 51 }
hazheng 59:b709711bc566 52
hazheng 59:b709711bc566 53 dataBuf = 0x00;
hazheng 59:b709711bc566 54 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 62:bc5caf59fe39 55 dataBuf = 0x00;
hazheng 62:bc5caf59fe39 56 imu_i2c_write_8bit(FXOS8700CQ_F_SETUP, &dataBuf);
hazheng 59:b709711bc566 57 dataBuf = 0x1F;
hazheng 59:b709711bc566 58 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 59 dataBuf = 0x20;
hazheng 59:b709711bc566 60 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG2, &dataBuf);
hazheng 62:bc5caf59fe39 61 dataBuf = FXOS8700CQ_XYZ_DATA_SC;
hazheng 59:b709711bc566 62 imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
hazheng 59:b709711bc566 63 dataBuf = 0x0D;
hazheng 59:b709711bc566 64 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 65
hazheng 62:bc5caf59fe39 66 return FXOS8700CQ_WHOAMI_VAL;
hazheng 59:b709711bc566 67 }
hazheng 59:b709711bc566 68
hazheng 59:b709711bc566 69 void imu_manager_update()
hazheng 59:b709711bc566 70 {
hazheng 59:b709711bc566 71 imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
hazheng 59:b709711bc566 72 // copy the 14 bit accelerometer byte data into 16 bit words
hazheng 62:bc5caf59fe39 73 static int16_t temp = 0;
hazheng 62:bc5caf59fe39 74 temp = ((((imu_data_buffer[1] << 8) | imu_data_buffer[2])) >> 2);
hazheng 62:bc5caf59fe39 75 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 76 {
hazheng 62:bc5caf59fe39 77 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 78 }
hazheng 62:bc5caf59fe39 79 accel_value.x = temp * ACCELER_SCALE_F;
hazheng 62:bc5caf59fe39 80
hazheng 62:bc5caf59fe39 81 temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
hazheng 62:bc5caf59fe39 82 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 83 {
hazheng 62:bc5caf59fe39 84 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 85 }
hazheng 62:bc5caf59fe39 86 accel_value.y = temp * ACCELER_SCALE_F;
hazheng 62:bc5caf59fe39 87
hazheng 62:bc5caf59fe39 88 temp = ((((imu_data_buffer[5] << 8) | imu_data_buffer[6])) >> 2);
hazheng 62:bc5caf59fe39 89 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 90 {
hazheng 62:bc5caf59fe39 91 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 92 }
hazheng 62:bc5caf59fe39 93 accel_value.z = temp * ACCELER_SCALE_F;
hazheng 62:bc5caf59fe39 94 /*
hazheng 59:b709711bc566 95 // copy the magnetometer byte data into 16 bit words
hazheng 62:bc5caf59fe39 96 magt_value.x = (((imu_data_buffer[7]) << 8) | imu_data_buffer[8]);
hazheng 62:bc5caf59fe39 97 magt_value.y = (((imu_data_buffer[9]) << 8) | imu_data_buffer[10]);
hazheng 62:bc5caf59fe39 98 magt_value.z = (((imu_data_buffer[11]) << 8) | imu_data_buffer[12]);
hazheng 62:bc5caf59fe39 99 */
hazheng 59:b709711bc566 100 }
hazheng 59:b709711bc566 101
hazheng 59:b709711bc566 102 const volatile struct imu_vec3* imu_manager_get_accl()
hazheng 59:b709711bc566 103 {
hazheng 59:b709711bc566 104 return &accel_value;
hazheng 59:b709711bc566 105 }
hazheng 59:b709711bc566 106
hazheng 59:b709711bc566 107 const volatile struct imu_vec3* imu_manager_get_magt()
hazheng 59:b709711bc566 108 {
hazheng 59:b709711bc566 109 return &magt_value;
hazheng 59:b709711bc566 110 }
hazheng 59:b709711bc566 111
hazheng 59:b709711bc566 112 #ifdef __cplusplus
hazheng 59:b709711bc566 113 }
hazheng 59:b709711bc566 114 #endif