Yihui Xiong / nrf51822_fix_i2c_spi_conflict

fix nrf51822 i2c & spi conflict

Dependencies:   BLE_API eMPL_MPU6050 nRF51822

Fork of Seeed_Tiny_BLE_Flash by Darren Huang

main.cpp@5:b8c02645e6af, 2015-11-17 (annotated)

Committer:
yihui
Date:
Tue Nov 17 07:48:56 2015 +0000
Revision:
5:b8c02645e6af
Parent:
4:19a0764d6b81 
fix i2c & spi conflict

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:26da608265f8 1
yihui 0:26da608265f8 2 #include "mbed.h"
yihui 0:26da608265f8 3 #include "mbed_i2c.h"
yihui 0:26da608265f8 4 #include "inv_mpu.h"
yihui 0:26da608265f8 5 #include "inv_mpu_dmp_motion_driver.h"
yihui 2:b61ddbb8528e 6 #include "nrf51.h"
yihui 2:b61ddbb8528e 7 #include "nrf51_bitfields.h"
yihui 0:26da608265f8 8
yihui 3:24e365bd1b97 9 #include "BLE.h"
yihui 0:26da608265f8 10 #include "DFUService.h"
yihui 0:26da608265f8 11 #include "UARTService.h"
yihui 0:26da608265f8 12
SOTB_DA 4:19a0764d6b81 13 #include "W25Q16BV.h"
SOTB_DA 4:19a0764d6b81 14 // flash
SOTB_DA 4:19a0764d6b81 15 DigitalOut vccFlash(p30, 1);
SOTB_DA 4:19a0764d6b81 16 #define PIN_SPI_MOSI p3
SOTB_DA 4:19a0764d6b81 17 #define PIN_SPI_MISO p5
SOTB_DA 4:19a0764d6b81 18 #define PIN_SPI_SCLK p4
SOTB_DA 4:19a0764d6b81 19 #define PIN_CS_FLASH p6
SOTB_DA 4:19a0764d6b81 20 W25Q16BV flash(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCLK, PIN_CS_FLASH); // mosi,miso,clk,cs
SOTB_DA 4:19a0764d6b81 21 int offsetAddr = 0;
SOTB_DA 4:19a0764d6b81 22 bool saveBufferFull = false;
yihui 0:26da608265f8 23
yihui 0:26da608265f8 24 #define LOG(...) { pc.printf(__VA_ARGS__); }
yihui 0:26da608265f8 25
yihui 0:26da608265f8 26 #define LED_GREEN p21
yihui 0:26da608265f8 27 #define LED_RED p22
yihui 0:26da608265f8 28 #define LED_BLUE p23
yihui 0:26da608265f8 29 #define BUTTON_PIN p17
yihui 0:26da608265f8 30 #define BATTERY_PIN p1
yihui 0:26da608265f8 31
yihui 0:26da608265f8 32 #define MPU6050_SDA p12
yihui 0:26da608265f8 33 #define MPU6050_SCL p13
yihui 0:26da608265f8 34
yihui 0:26da608265f8 35 #define UART_TX p9
yihui 0:26da608265f8 36 #define UART_RX p11
yihui 0:26da608265f8 37 #define UART_CTS p8
yihui 0:26da608265f8 38 #define UART_RTS p10
yihui 0:26da608265f8 39
yihui 0:26da608265f8 40 /* Starting sampling rate. */
SOTB_DA 4:19a0764d6b81 41 #define DEFAULT_MPU_HZ (200)
yihui 0:26da608265f8 42
yihui 0:26da608265f8 43 DigitalOut blue(LED_BLUE);
yihui 0:26da608265f8 44 DigitalOut green(LED_GREEN);
yihui 0:26da608265f8 45 DigitalOut red(LED_RED);
yihui 0:26da608265f8 46
yihui 0:26da608265f8 47 InterruptIn button(BUTTON_PIN);
yihui 0:26da608265f8 48 AnalogIn battery(BATTERY_PIN);
yihui 0:26da608265f8 49 Serial pc(UART_TX, UART_RX);
yihui 0:26da608265f8 50
yihui 0:26da608265f8 51 InterruptIn motion_probe(p14);
yihui 0:26da608265f8 52
yihui 2:b61ddbb8528e 53 int read_none_count = 0;
yihui 2:b61ddbb8528e 54
yihui 0:26da608265f8 55 BLEDevice ble;
yihui 0:26da608265f8 56 UARTService *uartServicePtr;
yihui 0:26da608265f8 57
yihui 0:26da608265f8 58 volatile bool bleIsConnected = false;
yihui 0:26da608265f8 59 volatile uint8_t tick_event = 0;
yihui 0:26da608265f8 60 volatile uint8_t motion_event = 0;
yihui 0:26da608265f8 61 static signed char board_orientation[9] = {
yihui 0:26da608265f8 62 1, 0, 0,
yihui 0:26da608265f8 63 0, 1, 0,
yihui 0:26da608265f8 64 0, 0, 1
yihui 0:26da608265f8 65 };
yihui 0:26da608265f8 66
yihui 0:26da608265f8 67
yihui 0:26da608265f8 68 void check_i2c_bus(void);
yihui 0:26da608265f8 69 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
yihui 0:26da608265f8 70
yihui 2:b61ddbb8528e 71
yihui 3:24e365bd1b97 72 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
yihui 0:26da608265f8 73 {
yihui 0:26da608265f8 74 LOG("Connected!\n");
yihui 0:26da608265f8 75 bleIsConnected = true;
yihui 0:26da608265f8 76 }
yihui 0:26da608265f8 77
yihui 3:24e365bd1b97 78 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
yihui 0:26da608265f8 79 {
yihui 0:26da608265f8 80 LOG("Disconnected!\n");
yihui 0:26da608265f8 81 LOG("Restarting the advertising process\n");
yihui 0:26da608265f8 82 ble.startAdvertising();
yihui 0:26da608265f8 83 bleIsConnected = false;
yihui 0:26da608265f8 84 }
yihui 0:26da608265f8 85
SOTB_DA 4:19a0764d6b81 86 //void tick(void)
SOTB_DA 4:19a0764d6b81 87 //{
SOTB_DA 4:19a0764d6b81 88 // static uint32_t count = 0;
SOTB_DA 4:19a0764d6b81 89 //
SOTB_DA 4:19a0764d6b81 90 // LOG("%d\r\n", count++);
SOTB_DA 4:19a0764d6b81 91 // green = !green;
SOTB_DA 4:19a0764d6b81 92 //}
yihui 0:26da608265f8 93
yihui 0:26da608265f8 94 void detect(void)
yihui 0:26da608265f8 95 {
yihui 2:b61ddbb8528e 96 LOG("Button pressed\n");
yihui 0:26da608265f8 97 blue = !blue;
yihui 0:26da608265f8 98 }
yihui 0:26da608265f8 99
yihui 0:26da608265f8 100 void motion_interrupt_handle(void)
yihui 0:26da608265f8 101 {
yihui 0:26da608265f8 102 motion_event = 1;
yihui 0:26da608265f8 103 }
yihui 0:26da608265f8 104
yihui 0:26da608265f8 105 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:26da608265f8 106 {
yihui 0:26da608265f8 107 LOG("Tap motion detected\n");
yihui 0:26da608265f8 108 }
yihui 0:26da608265f8 109
yihui 0:26da608265f8 110 void android_orient_cb(unsigned char orientation)
yihui 0:26da608265f8 111 {
yihui 0:26da608265f8 112 LOG("Oriention changed\n");
yihui 0:26da608265f8 113 }
yihui 0:26da608265f8 114
yihui 0:26da608265f8 115
SOTB_DA 4:19a0764d6b81 116 void saveMPUDataToFlash(int addr, unsigned long timestamp, short accel[3], short gyro[3], long quat[4])
SOTB_DA 4:19a0764d6b81 117 {
SOTB_DA 4:19a0764d6b81 118 uint8_t mpuRawData[32] = {0,};
SOTB_DA 4:19a0764d6b81 119 mpuRawData[0] = timestamp;
SOTB_DA 4:19a0764d6b81 120 mpuRawData[1] = timestamp>>8;
SOTB_DA 4:19a0764d6b81 121 mpuRawData[2] = timestamp>>16;
SOTB_DA 4:19a0764d6b81 122 mpuRawData[3] = timestamp>>24;
SOTB_DA 4:19a0764d6b81 123 mpuRawData[4] = accel[0];
SOTB_DA 4:19a0764d6b81 124 mpuRawData[5] = accel[0]>>8;
SOTB_DA 4:19a0764d6b81 125 mpuRawData[6] = accel[1];
SOTB_DA 4:19a0764d6b81 126 mpuRawData[7] = accel[1]>>8;
SOTB_DA 4:19a0764d6b81 127 mpuRawData[8] = accel[2];
SOTB_DA 4:19a0764d6b81 128 mpuRawData[9] = accel[2]>>8;
SOTB_DA 4:19a0764d6b81 129 mpuRawData[10] = gyro[0];
SOTB_DA 4:19a0764d6b81 130 mpuRawData[11] = gyro[0]>>8;
SOTB_DA 4:19a0764d6b81 131 mpuRawData[12] = gyro[1];
SOTB_DA 4:19a0764d6b81 132 mpuRawData[13] = gyro[1]>>8;
SOTB_DA 4:19a0764d6b81 133 mpuRawData[14] = gyro[2];
SOTB_DA 4:19a0764d6b81 134 mpuRawData[15] = gyro[2]>>8;
SOTB_DA 4:19a0764d6b81 135 mpuRawData[16] = quat[0];
SOTB_DA 4:19a0764d6b81 136 mpuRawData[17] = quat[0]>>8;
SOTB_DA 4:19a0764d6b81 137 mpuRawData[18] = quat[0]>>16;
SOTB_DA 4:19a0764d6b81 138 mpuRawData[19] = quat[0]>>24;
SOTB_DA 4:19a0764d6b81 139 mpuRawData[20] = quat[1];
SOTB_DA 4:19a0764d6b81 140 mpuRawData[21] = quat[1]>>8;
SOTB_DA 4:19a0764d6b81 141 mpuRawData[22] = quat[1]>>16;
SOTB_DA 4:19a0764d6b81 142 mpuRawData[23] = quat[1]>>24;
SOTB_DA 4:19a0764d6b81 143 mpuRawData[24] = quat[2];
SOTB_DA 4:19a0764d6b81 144 mpuRawData[25] = quat[2]>>8;
SOTB_DA 4:19a0764d6b81 145 mpuRawData[26] = quat[2]>>16;
SOTB_DA 4:19a0764d6b81 146 mpuRawData[27] = quat[2]>>24;
SOTB_DA 4:19a0764d6b81 147 mpuRawData[28] = quat[3];
SOTB_DA 4:19a0764d6b81 148 mpuRawData[29] = quat[3]>>8;
SOTB_DA 4:19a0764d6b81 149 mpuRawData[30] = quat[3]>>16;
SOTB_DA 4:19a0764d6b81 150 mpuRawData[31] = quat[3]>>24;
SOTB_DA 4:19a0764d6b81 151 flash.writeStream(addr, (char *)mpuRawData, 32);
SOTB_DA 4:19a0764d6b81 152 pc.printf("%02x %02x %02x %02x\r\n", mpuRawData[0], mpuRawData[1], mpuRawData[2], mpuRawData[3]);
SOTB_DA 4:19a0764d6b81 153 }
SOTB_DA 4:19a0764d6b81 154
yihui 0:26da608265f8 155 int main(void)
yihui 0:26da608265f8 156 {
yihui 0:26da608265f8 157 blue = 1;
yihui 0:26da608265f8 158 green = 1;
yihui 0:26da608265f8 159 red = 1;
yihui 0:26da608265f8 160
yihui 0:26da608265f8 161 pc.baud(115200);
yihui 2:b61ddbb8528e 162
SOTB_DA 4:19a0764d6b81 163 pc.printf("SPI init done\n");
SOTB_DA 4:19a0764d6b81 164 flash.chipErase();
SOTB_DA 4:19a0764d6b81 165 pc.printf("Flash Erase done\n");
SOTB_DA 4:19a0764d6b81 166 uint8_t buff[128];
SOTB_DA 4:19a0764d6b81 167 pc.printf("write after erase\r\n");
SOTB_DA 4:19a0764d6b81 168 uint8_t datas[100] = {0,};
SOTB_DA 4:19a0764d6b81 169 for (uint8_t i=0; i<100;i++) {
SOTB_DA 4:19a0764d6b81 170 datas[i] = i+0x01;
SOTB_DA 4:19a0764d6b81 171 }
SOTB_DA 4:19a0764d6b81 172 long dfa = 32524;
SOTB_DA 4:19a0764d6b81 173 datas[0] = dfa;
SOTB_DA 4:19a0764d6b81 174 datas[1] = dfa >> 8;
SOTB_DA 4:19a0764d6b81 175 datas[2] = dfa >> 16;
SOTB_DA 4:19a0764d6b81 176 datas[3] = dfa >> 24;
SOTB_DA 4:19a0764d6b81 177 flash.writeStream(100, (char *)datas, 100);
SOTB_DA 4:19a0764d6b81 178
SOTB_DA 4:19a0764d6b81 179 pc.printf("read after write\r\n");
SOTB_DA 4:19a0764d6b81 180 flash.readStream(100, (char*)buff, 100);
SOTB_DA 4:19a0764d6b81 181 for (int i=0; i<100; i++) {
SOTB_DA 4:19a0764d6b81 182 pc.printf("%02x", buff[i]);
SOTB_DA 4:19a0764d6b81 183 }
SOTB_DA 4:19a0764d6b81 184 pc.printf("\r\n");
SOTB_DA 4:19a0764d6b81 185
SOTB_DA 4:19a0764d6b81 186 pc.printf("%d\r\n", flash.readByte(0x34));
SOTB_DA 4:19a0764d6b81 187 int tmp = 66;
SOTB_DA 4:19a0764d6b81 188 pc.printf("%d\r\n", flash.readByte(tmp));
SOTB_DA 4:19a0764d6b81 189 // read stream from 0x168
SOTB_DA 4:19a0764d6b81 190 char str2[11] = {0};
SOTB_DA 4:19a0764d6b81 191 flash.readStream(tmp, str2, 11);
SOTB_DA 4:19a0764d6b81 192 pc.printf("%s\n",str2);
SOTB_DA 4:19a0764d6b81 193
SOTB_DA 4:19a0764d6b81 194 for (int i=50; i< 150;i++){
SOTB_DA 4:19a0764d6b81 195 pc.printf("%02x", flash.readByte(i));
SOTB_DA 4:19a0764d6b81 196 }
SOTB_DA 4:19a0764d6b81 197 pc.printf("\r\n");
SOTB_DA 4:19a0764d6b81 198
SOTB_DA 4:19a0764d6b81 199 char string[] = "ABCDEFGHIJK";
SOTB_DA 4:19a0764d6b81 200 flash.writeStream(tmp, string, 11);
SOTB_DA 4:19a0764d6b81 201
SOTB_DA 4:19a0764d6b81 202 char str3[11] = {0};
SOTB_DA 4:19a0764d6b81 203 flash.readStream(tmp, str3, 11);
SOTB_DA 4:19a0764d6b81 204 pc.printf("%s\n",str3);
SOTB_DA 4:19a0764d6b81 205
yihui 2:b61ddbb8528e 206 wait(1);
yihui 2:b61ddbb8528e 207
yihui 0:26da608265f8 208 LOG("---- Seeed Tiny BLE ----\n");
yihui 0:26da608265f8 209
yihui 2:b61ddbb8528e 210 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 211 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 212
yihui 0:26da608265f8 213
yihui 0:26da608265f8 214 if (mpu_init(0)) {
yihui 2:b61ddbb8528e 215 LOG("failed to initialize mpu6050\r\n");
yihui 0:26da608265f8 216 }
yihui 0:26da608265f8 217
yihui 0:26da608265f8 218 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 219 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 220 mpu_set_sample_rate(DEFAULT_MPU_HZ);
SOTB_DA 4:19a0764d6b81 221 mpu_set_gyro_fsr(2000);
SOTB_DA 4:19a0764d6b81 222 mpu_set_accel_fsr(16);
yihui 0:26da608265f8 223 dmp_load_motion_driver_firmware();
yihui 0:26da608265f8 224 dmp_set_orientation(
yihui 0:26da608265f8 225 inv_orientation_matrix_to_scalar(board_orientation));
yihui 0:26da608265f8 226 dmp_register_tap_cb(tap_cb);
yihui 0:26da608265f8 227 dmp_register_android_orient_cb(android_orient_cb);
yihui 0:26da608265f8 228 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
SOTB_DA 4:19a0764d6b81 229 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL |
SOTB_DA 4:19a0764d6b81 230 DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
yihui 0:26da608265f8 231 dmp_enable_feature(dmp_features);
yihui 0:26da608265f8 232 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 233 mpu_set_dmp_state(1);
yihui 0:26da608265f8 234 dmp_set_tap_thresh(TAP_XYZ, 50);
yihui 2:b61ddbb8528e 235
yihui 2:b61ddbb8528e 236
yihui 2:b61ddbb8528e 237 motion_probe.fall(motion_interrupt_handle);
yihui 0:26da608265f8 238
yihui 2:b61ddbb8528e 239 button.fall(detect);
yihui 2:b61ddbb8528e 240
yihui 2:b61ddbb8528e 241 LOG("Initialising the nRF51822\n");
yihui 2:b61ddbb8528e 242 ble.init();
yihui 3:24e365bd1b97 243 ble.gap().onDisconnection(disconnectionCallback);
yihui 3:24e365bd1b97 244 ble.gap().onConnection(connectionCallback);
yihui 2:b61ddbb8528e 245
yihui 2:b61ddbb8528e 246
yihui 2:b61ddbb8528e 247 /* setup advertising */
yihui 2:b61ddbb8528e 248 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
yihui 2:b61ddbb8528e 249 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
yihui 2:b61ddbb8528e 250 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
yihui 2:b61ddbb8528e 251 (const uint8_t *)"smurfs", sizeof("smurfs"));
yihui 2:b61ddbb8528e 252 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
yihui 2:b61ddbb8528e 253 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
yihui 2:b61ddbb8528e 254 DFUService dfu(ble);
yihui 2:b61ddbb8528e 255 UARTService uartService(ble);
yihui 2:b61ddbb8528e 256 uartServicePtr = &uartService;
yihui 2:b61ddbb8528e 257 //uartService.retargetStdout();
yihui 2:b61ddbb8528e 258
yihui 2:b61ddbb8528e 259 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
yihui 3:24e365bd1b97 260 ble.gap().startAdvertising();
yihui 2:b61ddbb8528e 261
SOTB_DA 4:19a0764d6b81 262
SOTB_DA 4:19a0764d6b81 263 uint8_t tmp1[32] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32};
SOTB_DA 4:19a0764d6b81 264 Timer timer;
SOTB_DA 4:19a0764d6b81 265 timer.start();
yihui 0:26da608265f8 266 while (true) {
yihui 0:26da608265f8 267 if (motion_event) {
yihui 3:24e365bd1b97 268
yihui 3:24e365bd1b97 269 unsigned long sensor_timestamp;
yihui 3:24e365bd1b97 270 short gyro[3], accel[3], sensors;
yihui 3:24e365bd1b97 271 long quat[4];
yihui 3:24e365bd1b97 272 unsigned char more = 1;
yihui 0:26da608265f8 273
yihui 3:24e365bd1b97 274 while (more) {
yihui 3:24e365bd1b97 275 /* This function gets new data from the FIFO when the DMP is in
yihui 3:24e365bd1b97 276 * use. The FIFO can contain any combination of gyro, accel,
yihui 3:24e365bd1b97 277 * quaternion, and gesture data. The sensors parameter tells the
yihui 3:24e365bd1b97 278 * caller which data fields were actually populated with new data.
yihui 3:24e365bd1b97 279 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 3:24e365bd1b97 280 * the FIFO isn't being filled with accel data.
yihui 3:24e365bd1b97 281 * The driver parses the gesture data to determine if a gesture
yihui 3:24e365bd1b97 282 * event has occurred; on an event, the application will be notified
yihui 3:24e365bd1b97 283 * via a callback (assuming that a callback function was properly
yihui 3:24e365bd1b97 284 * registered). The more parameter is non-zero if there are
yihui 3:24e365bd1b97 285 * leftover packets in the FIFO.
yihui 3:24e365bd1b97 286 */
yihui 3:24e365bd1b97 287 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
yihui 3:24e365bd1b97 288 &more);
yihui 3:24e365bd1b97 289
SOTB_DA 4:19a0764d6b81 290 // saveMPUDataToFlash(offsetAddr, sensor_timestamp, accel, gyro, quat);
SOTB_DA 4:19a0764d6b81 291 // char tmpe[32];
SOTB_DA 4:19a0764d6b81 292 // flash.readStream(offsetAddr, tmpe, 32);
SOTB_DA 4:19a0764d6b81 293 // pc.printf("%02x%02x%02x%02x\r\n", tmpe[0],tmpe[1],tmpe[2],tmpe[3]);
yihui 3:24e365bd1b97 294
yihui 3:24e365bd1b97 295 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 3:24e365bd1b97 296 * frame and hardware units. This behavior is convenient because it
yihui 3:24e365bd1b97 297 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 3:24e365bd1b97 298 * mpu_read_fifo consistent.
yihui 3:24e365bd1b97 299 */
yihui 3:24e365bd1b97 300 if (sensors & INV_XYZ_GYRO) {
SOTB_DA 4:19a0764d6b81 301 LOG("time:%d, GYRO: %d, %d, %d\n", timer.read_ms(), gyro[0], gyro[1], gyro[2]);
yihui 3:24e365bd1b97 302 }
yihui 3:24e365bd1b97 303 if (sensors & INV_XYZ_ACCEL) {
yihui 3:24e365bd1b97 304 //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
yihui 3:24e365bd1b97 305 }
yihui 3:24e365bd1b97 306
yihui 3:24e365bd1b97 307 /* Unlike gyro and accel, quaternions are written to the FIFO in
yihui 3:24e365bd1b97 308 * the body frame, q30. The orientation is set by the scalar passed
yihui 3:24e365bd1b97 309 * to dmp_set_orientation during initialization.
yihui 3:24e365bd1b97 310 */
yihui 3:24e365bd1b97 311 if (sensors & INV_WXYZ_QUAT) {
yihui 3:24e365bd1b97 312 // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
yihui 3:24e365bd1b97 313 }
yihui 3:24e365bd1b97 314
yihui 3:24e365bd1b97 315 if (sensors) {
yihui 3:24e365bd1b97 316 read_none_count = 0;
yihui 3:24e365bd1b97 317 } else {
yihui 3:24e365bd1b97 318 read_none_count++;
yihui 3:24e365bd1b97 319 if (read_none_count > 3) {
yihui 3:24e365bd1b97 320 read_none_count = 0;
yihui 3:24e365bd1b97 321
yihui 3:24e365bd1b97 322 LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
yihui 3:24e365bd1b97 323 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 3:24e365bd1b97 324 }
yihui 3:24e365bd1b97 325 }
yihui 3:24e365bd1b97 326 }
SOTB_DA 4:19a0764d6b81 327 // test flash
SOTB_DA 4:19a0764d6b81 328 flash.writeStream(offsetAddr, (char *)tmp1, 32);
SOTB_DA 4:19a0764d6b81 329 char tmpe[32];
SOTB_DA 4:19a0764d6b81 330 flash.readStream(offsetAddr, tmpe, 32);
SOTB_DA 4:19a0764d6b81 331 for (int i=0; i<32; i++) {
SOTB_DA 4:19a0764d6b81 332 pc.printf("%02x", tmpe[i]);
SOTB_DA 4:19a0764d6b81 333 tmp1[i] += 1;
SOTB_DA 4:19a0764d6b81 334 }
SOTB_DA 4:19a0764d6b81 335 pc.printf("\r\n");
SOTB_DA 4:19a0764d6b81 336 offsetAddr += 32;
SOTB_DA 4:19a0764d6b81 337 if (offsetAddr >= 0xFFF) {
SOTB_DA 4:19a0764d6b81 338 printf("one turn\r\n");
SOTB_DA 4:19a0764d6b81 339 offsetAddr = 0;
SOTB_DA 4:19a0764d6b81 340 }
yihui 3:24e365bd1b97 341 motion_event = 0;
yihui 0:26da608265f8 342 } else {
yihui 0:26da608265f8 343 ble.waitForEvent();
yihui 0:26da608265f8 344 }
yihui 0:26da608265f8 345 }
yihui 0:26da608265f8 346 }
yihui 0:26da608265f8 347
yihui 0:26da608265f8 348 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 349 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 350 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 351 */
yihui 0:26da608265f8 352 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 353 {
yihui 0:26da608265f8 354 unsigned short b;
yihui 0:26da608265f8 355
yihui 0:26da608265f8 356 if (row[0] > 0)
yihui 0:26da608265f8 357 b = 0;
yihui 0:26da608265f8 358 else if (row[0] < 0)
yihui 0:26da608265f8 359 b = 4;
yihui 0:26da608265f8 360 else if (row[1] > 0)
yihui 0:26da608265f8 361 b = 1;
yihui 0:26da608265f8 362 else if (row[1] < 0)
yihui 0:26da608265f8 363 b = 5;
yihui 0:26da608265f8 364 else if (row[2] > 0)
yihui 0:26da608265f8 365 b = 2;
yihui 0:26da608265f8 366 else if (row[2] < 0)
yihui 0:26da608265f8 367 b = 6;
yihui 0:26da608265f8 368 else
yihui 0:26da608265f8 369 b = 7; // error
yihui 0:26da608265f8 370 return b;
yihui 0:26da608265f8 371 }
yihui 0:26da608265f8 372
yihui 0:26da608265f8 373 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 374 const signed char *mtx)
yihui 0:26da608265f8 375 {
yihui 0:26da608265f8 376 unsigned short scalar;
yihui 0:26da608265f8 377
yihui 0:26da608265f8 378 /*
yihui 0:26da608265f8 379 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 380 XZY 001_010_000
yihui 0:26da608265f8 381 YXZ 010_000_001
yihui 0:26da608265f8 382 YZX 000_010_001
yihui 0:26da608265f8 383 ZXY 001_000_010
yihui 0:26da608265f8 384 ZYX 000_001_010
yihui 0:26da608265f8 385 */
yihui 0:26da608265f8 386
yihui 0:26da608265f8 387 scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 388 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 389 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 390
yihui 0:26da608265f8 391
yihui 0:26da608265f8 392 return scalar;
yihui 0:26da608265f8 393 }
yihui 0:26da608265f8 394