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

RemovedSources/IMUManager.cpp.txt@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
87:15fcf7891bf9 
Better version of Intersection detection.

Who changed what in which revision?

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