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@64:43ab429a37e0, 2017-04-10 (annotated)

Committer:
hazheng
Date:
Mon Apr 10 16:44:31 2017 +0000
Revision:
64:43ab429a37e0
Parent:
63:d9a81b3d69f5 
Auto terminate when hit the end of the loop. Commented out all the IMU code, because of the memory space problem.

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