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

Committer:
hazheng
Date:
Sun Apr 09 22:08:34 2017 +0000
Revision:
63:d9a81b3d69f5
Parent:
62:bc5caf59fe39
Child:
64:43ab429a37e0
Finished the code for Accelerometer.

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 63:d9a81b3d69f5 3 #include "math.h"
hazheng 59:b709711bc566 4
hazheng 62:bc5caf59fe39 5 //#define SW_DEBUG
hazheng 62:bc5caf59fe39 6 #include "SWCommon.h"
hazheng 62:bc5caf59fe39 7
hazheng 59:b709711bc566 8 #ifdef __cplusplus
hazheng 59:b709711bc566 9 extern "C" {
hazheng 59:b709711bc566 10 #endif
hazheng 59:b709711bc566 11
hazheng 63:d9a81b3d69f5 12 #define KEEP_TWO_DECIMAL(X) ((float)((int)(X * 100)) / 100)
hazheng 63:d9a81b3d69f5 13
hazheng 59:b709711bc566 14 static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
hazheng 59:b709711bc566 15 static const int SLAVE_ADDR_READ = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;
hazheng 63:d9a81b3d69f5 16 static const float ACCELER_SCALE_F_G = ACCELER_SCALE_F_MG * 0.001f;
hazheng 59:b709711bc566 17
hazheng 59:b709711bc566 18 static volatile struct imu_vec3 accel_value;
hazheng 63:d9a81b3d69f5 19 static volatile struct imu_vec3 accel_offset;
hazheng 63:d9a81b3d69f5 20 static volatile struct imu_vec3 velocity_value;
hazheng 63:d9a81b3d69f5 21 static volatile struct imu_vec3 position_value;
hazheng 63:d9a81b3d69f5 22 //static volatile struct imu_vec3 magt_value;
hazheng 59:b709711bc566 23 static I2C imu_i2c_port(PIN_IMC_SDA, PIN_IMC_SCL);
hazheng 59:b709711bc566 24 static DigitalOut imu_accl_sa0(PIN_IMC_ACCL_SA0, ACCEL_MAG_SA0);
hazheng 59:b709711bc566 25 static DigitalOut imu_accl_sa1(PIN_IMC_ACCL_SA1, ACCEL_MAG_SA1);
hazheng 62:bc5caf59fe39 26 static char imu_data_buffer[FXOS8700CQ_READ_LEN] = { 0 };
hazheng 63:d9a81b3d69f5 27 static Ticker m_imu_update_tick;
hazheng 63:d9a81b3d69f5 28 static Timer m_imu_update_timer;
hazheng 63:d9a81b3d69f5 29 static float m_imu_update_prev_time;
hazheng 63:d9a81b3d69f5 30
hazheng 63:d9a81b3d69f5 31 //Debug
hazheng 63:d9a81b3d69f5 32 static DebugCounter counter(10, PTE4);
hazheng 59:b709711bc566 33
hazheng 59:b709711bc566 34 inline void imu_i2c_write_8bit(const char addr, const char* buffer)
hazheng 59:b709711bc566 35 {
hazheng 62:bc5caf59fe39 36 static char write_buf[2];
hazheng 62:bc5caf59fe39 37 write_buf[0] = addr;
hazheng 62:bc5caf59fe39 38 write_buf[1] = *buffer;
hazheng 62:bc5caf59fe39 39 imu_i2c_port.write(SLAVE_ADDR_WRITE, write_buf, 2, false);
hazheng 59:b709711bc566 40 }
hazheng 59:b709711bc566 41
hazheng 59:b709711bc566 42 inline void imu_i2c_read_8bit(const char addr, char* buffer)
hazheng 59:b709711bc566 43 {
hazheng 62:bc5caf59fe39 44 int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 62:bc5caf59fe39 45 int t2 = imu_i2c_port.read( SLAVE_ADDR_READ, buffer, 1, false);
hazheng 59:b709711bc566 46 }
hazheng 59:b709711bc566 47
hazheng 59:b709711bc566 48 inline void imu_i2c_read(const char addr, char* buffer, const int length)
hazheng 59:b709711bc566 49 {
hazheng 62:bc5caf59fe39 50 int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr, 1, true);
hazheng 62:bc5caf59fe39 51 int t2 = imu_i2c_port.read( SLAVE_ADDR_READ, buffer, length, false);
hazheng 59:b709711bc566 52 }
hazheng 59:b709711bc566 53
hazheng 59:b709711bc566 54 uint8_t imu_manager_init()
hazheng 59:b709711bc566 55 {
hazheng 62:bc5caf59fe39 56 accel_value.x = accel_value.y = accel_value.z = 0;
hazheng 63:d9a81b3d69f5 57 velocity_value.x = velocity_value.y = velocity_value.z = 0;
hazheng 63:d9a81b3d69f5 58 position_value.x = position_value.y = position_value.z = 0;
hazheng 62:bc5caf59fe39 59
hazheng 59:b709711bc566 60 char dataBuf = 0;
hazheng 59:b709711bc566 61 imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
hazheng 59:b709711bc566 62 if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
hazheng 59:b709711bc566 63 {
hazheng 62:bc5caf59fe39 64 return dataBuf;
hazheng 59:b709711bc566 65 }
hazheng 63:d9a81b3d69f5 66 //standby
hazheng 59:b709711bc566 67 dataBuf = 0x00;
hazheng 59:b709711bc566 68 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 63:d9a81b3d69f5 69 //reset
hazheng 63:d9a81b3d69f5 70 dataBuf = FXOS8700CQ_RESET_MASK;
hazheng 63:d9a81b3d69f5 71 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG2, &dataBuf);
hazheng 63:d9a81b3d69f5 72 wait(0.5);
hazheng 63:d9a81b3d69f5 73 //standby
hazheng 63:d9a81b3d69f5 74 dataBuf = 0x00;
hazheng 63:d9a81b3d69f5 75 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 63:d9a81b3d69f5 76
hazheng 63:d9a81b3d69f5 77 dataBuf = 0x09;
hazheng 63:d9a81b3d69f5 78 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG2, &dataBuf);
hazheng 62:bc5caf59fe39 79 dataBuf = 0x00;
hazheng 62:bc5caf59fe39 80 imu_i2c_write_8bit(FXOS8700CQ_F_SETUP, &dataBuf);
hazheng 59:b709711bc566 81 dataBuf = 0x1F;
hazheng 59:b709711bc566 82 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 83 dataBuf = 0x20;
hazheng 59:b709711bc566 84 imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG2, &dataBuf);
hazheng 62:bc5caf59fe39 85 dataBuf = FXOS8700CQ_XYZ_DATA_SC;
hazheng 59:b709711bc566 86 imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
hazheng 63:d9a81b3d69f5 87 dataBuf = FXOS8700CQ_CTRL_REG1_v;
hazheng 59:b709711bc566 88 imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 59:b709711bc566 89
hazheng 62:bc5caf59fe39 90 return FXOS8700CQ_WHOAMI_VAL;
hazheng 59:b709711bc566 91 }
hazheng 59:b709711bc566 92
hazheng 63:d9a81b3d69f5 93 void imu_manager_calibrate()
hazheng 63:d9a81b3d69f5 94 {
hazheng 63:d9a81b3d69f5 95
hazheng 63:d9a81b3d69f5 96 static const int calibrate_sample_num = 10;
hazheng 63:d9a81b3d69f5 97
hazheng 63:d9a81b3d69f5 98
hazheng 63:d9a81b3d69f5 99 int16_t temp = 0;
hazheng 63:d9a81b3d69f5 100 float avgX = 0.0f;
hazheng 63:d9a81b3d69f5 101 float avgY = 0.0f;
hazheng 63:d9a81b3d69f5 102
hazheng 63:d9a81b3d69f5 103 for(int i = 0; i < calibrate_sample_num; ++i)
hazheng 63:d9a81b3d69f5 104 {
hazheng 63:d9a81b3d69f5 105 imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
hazheng 63:d9a81b3d69f5 106 temp = ((((imu_data_buffer[1] << 8) | imu_data_buffer[2])) >> 2);
hazheng 63:d9a81b3d69f5 107 if(temp & 0x2000)
hazheng 63:d9a81b3d69f5 108 {
hazheng 63:d9a81b3d69f5 109 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 63:d9a81b3d69f5 110
hazheng 63:d9a81b3d69f5 111 }
hazheng 63:d9a81b3d69f5 112 avgX += (static_cast<float>(temp) * ACCELER_SCALE_F_MG);
hazheng 63:d9a81b3d69f5 113 //LOGI("I: %5.3f, %d ", avgX, temp);
hazheng 63:d9a81b3d69f5 114 //wait(5.0f);
hazheng 63:d9a81b3d69f5 115 temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
hazheng 63:d9a81b3d69f5 116 if(temp & 0x2000)
hazheng 63:d9a81b3d69f5 117 {
hazheng 63:d9a81b3d69f5 118 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 63:d9a81b3d69f5 119 }
hazheng 63:d9a81b3d69f5 120 avgY += (static_cast<float>(temp) * ACCELER_SCALE_F_MG);
hazheng 63:d9a81b3d69f5 121 wait(IMU_UPDATE_TICK_RATE * 2);
hazheng 63:d9a81b3d69f5 122 }
hazheng 63:d9a81b3d69f5 123 //Standby mode
hazheng 63:d9a81b3d69f5 124 //char dataBuf = 0x00;
hazheng 63:d9a81b3d69f5 125 //imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 63:d9a81b3d69f5 126
hazheng 63:d9a81b3d69f5 127 avgX = avgX / calibrate_sample_num;
hazheng 63:d9a81b3d69f5 128 avgY = avgY / calibrate_sample_num;
hazheng 63:d9a81b3d69f5 129 accel_offset.x = avgX * -0.001;
hazheng 63:d9a81b3d69f5 130 accel_offset.y = avgY * -0.001;
hazheng 63:d9a81b3d69f5 131 accel_offset.z = 0;
hazheng 63:d9a81b3d69f5 132 /*
hazheng 63:d9a81b3d69f5 133 dataBuf = static_cast<char>((abs(avgX)) / OFFSET_SCALE_F);
hazheng 63:d9a81b3d69f5 134 if(avgX > 0)
hazheng 63:d9a81b3d69f5 135 {
hazheng 63:d9a81b3d69f5 136 dataBuf = (~dataBuf) + 1;
hazheng 63:d9a81b3d69f5 137 }
hazheng 63:d9a81b3d69f5 138 //imu_i2c_write_8bit(FXOS8700CQ_OFF_X, &dataBuf);
hazheng 63:d9a81b3d69f5 139 //LOGI("I: %5.3f, %d ", avgX, dataBuf);
hazheng 63:d9a81b3d69f5 140 //wait(5.0f);
hazheng 63:d9a81b3d69f5 141
hazheng 63:d9a81b3d69f5 142 dataBuf = static_cast<char>((abs(avgY)) / OFFSET_SCALE_F);
hazheng 63:d9a81b3d69f5 143 if(avgY > 0)
hazheng 63:d9a81b3d69f5 144 {
hazheng 63:d9a81b3d69f5 145 dataBuf = (~dataBuf) + 1;
hazheng 63:d9a81b3d69f5 146 }
hazheng 63:d9a81b3d69f5 147 //imu_i2c_write_8bit(FXOS8700CQ_OFF_Y, &dataBuf);
hazheng 63:d9a81b3d69f5 148 */
hazheng 63:d9a81b3d69f5 149 //Avtive mode.
hazheng 63:d9a81b3d69f5 150 //dataBuf = FXOS8700CQ_CTRL_REG1_v;
hazheng 63:d9a81b3d69f5 151 //imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
hazheng 63:d9a81b3d69f5 152 }
hazheng 63:d9a81b3d69f5 153
hazheng 63:d9a81b3d69f5 154 void imu_manager_begin_tick()
hazheng 63:d9a81b3d69f5 155 {
hazheng 63:d9a81b3d69f5 156 m_imu_update_prev_time = 0.0f;
hazheng 63:d9a81b3d69f5 157 m_imu_update_tick.attach(&imu_manager_update, IMU_UPDATE_TICK_RATE);
hazheng 63:d9a81b3d69f5 158 m_imu_update_timer.start();
hazheng 63:d9a81b3d69f5 159 }
hazheng 63:d9a81b3d69f5 160
hazheng 59:b709711bc566 161 void imu_manager_update()
hazheng 59:b709711bc566 162 {
hazheng 63:d9a81b3d69f5 163 float currentTime = m_imu_update_timer.read();
hazheng 63:d9a81b3d69f5 164 float deltaTime = currentTime - m_imu_update_prev_time;
hazheng 63:d9a81b3d69f5 165 m_imu_update_prev_time = currentTime;
hazheng 63:d9a81b3d69f5 166
hazheng 59:b709711bc566 167 imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
hazheng 59:b709711bc566 168 // copy the 14 bit accelerometer byte data into 16 bit words
hazheng 62:bc5caf59fe39 169 static int16_t temp = 0;
hazheng 62:bc5caf59fe39 170 temp = ((((imu_data_buffer[1] << 8) | imu_data_buffer[2])) >> 2);
hazheng 62:bc5caf59fe39 171 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 172 {
hazheng 62:bc5caf59fe39 173 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 174 }
hazheng 63:d9a81b3d69f5 175 accel_value.x = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.x;
hazheng 63:d9a81b3d69f5 176 accel_value.x = KEEP_TWO_DECIMAL(accel_value.x);
hazheng 62:bc5caf59fe39 177
hazheng 62:bc5caf59fe39 178 temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
hazheng 62:bc5caf59fe39 179 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 180 {
hazheng 62:bc5caf59fe39 181 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 182 }
hazheng 63:d9a81b3d69f5 183 accel_value.y = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.y;
hazheng 63:d9a81b3d69f5 184 accel_value.y = KEEP_TWO_DECIMAL(accel_value.y);
hazheng 62:bc5caf59fe39 185
hazheng 62:bc5caf59fe39 186 temp = ((((imu_data_buffer[5] << 8) | imu_data_buffer[6])) >> 2);
hazheng 62:bc5caf59fe39 187 if(temp & 0x2000)
hazheng 62:bc5caf59fe39 188 {
hazheng 62:bc5caf59fe39 189 temp = -1 * ((~(temp | 0xC000)) + 1);
hazheng 62:bc5caf59fe39 190 }
hazheng 63:d9a81b3d69f5 191 accel_value.z = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.z;
hazheng 63:d9a81b3d69f5 192 accel_value.z = KEEP_TWO_DECIMAL(accel_value.z);
hazheng 62:bc5caf59fe39 193 /*
hazheng 59:b709711bc566 194 // copy the magnetometer byte data into 16 bit words
hazheng 62:bc5caf59fe39 195 magt_value.x = (((imu_data_buffer[7]) << 8) | imu_data_buffer[8]);
hazheng 62:bc5caf59fe39 196 magt_value.y = (((imu_data_buffer[9]) << 8) | imu_data_buffer[10]);
hazheng 62:bc5caf59fe39 197 magt_value.z = (((imu_data_buffer[11]) << 8) | imu_data_buffer[12]);
hazheng 62:bc5caf59fe39 198 */
hazheng 63:d9a81b3d69f5 199
hazheng 63:d9a81b3d69f5 200 velocity_value.x = velocity_value.x + (accel_value.x * IMU_DEFAULT_G * deltaTime);
hazheng 63:d9a81b3d69f5 201 velocity_value.y = velocity_value.y + (accel_value.y * IMU_DEFAULT_G * deltaTime);
hazheng 63:d9a81b3d69f5 202
hazheng 63:d9a81b3d69f5 203 position_value.x = position_value.x + (velocity_value.x * deltaTime);
hazheng 63:d9a81b3d69f5 204 position_value.y = position_value.y + (velocity_value.y * deltaTime);
hazheng 63:d9a81b3d69f5 205
hazheng 63:d9a81b3d69f5 206 counter.Update();
hazheng 59:b709711bc566 207 }
hazheng 59:b709711bc566 208
hazheng 59:b709711bc566 209 const volatile struct imu_vec3* imu_manager_get_accl()
hazheng 59:b709711bc566 210 {
hazheng 59:b709711bc566 211 return &accel_value;
hazheng 59:b709711bc566 212 }
hazheng 63:d9a81b3d69f5 213 /*
hazheng 59:b709711bc566 214 const volatile struct imu_vec3* imu_manager_get_magt()
hazheng 59:b709711bc566 215 {
hazheng 59:b709711bc566 216 return &magt_value;
hazheng 59:b709711bc566 217 }
hazheng 63:d9a81b3d69f5 218 */
hazheng 63:d9a81b3d69f5 219
hazheng 63:d9a81b3d69f5 220 const volatile struct imu_vec3* imu_manager_get_velocity()
hazheng 63:d9a81b3d69f5 221 {
hazheng 63:d9a81b3d69f5 222 return &velocity_value;
hazheng 63:d9a81b3d69f5 223 }
hazheng 63:d9a81b3d69f5 224
hazheng 63:d9a81b3d69f5 225 const volatile struct imu_vec3* imu_manager_get_position()
hazheng 63:d9a81b3d69f5 226 {
hazheng 63:d9a81b3d69f5 227 return &position_value;
hazheng 63:d9a81b3d69f5 228 }
hazheng 59:b709711bc566 229
hazheng 59:b709711bc566 230 #ifdef __cplusplus
hazheng 59:b709711bc566 231 }
hazheng 59:b709711bc566 232 #endif