main.cpp

00001 
00002 #include "mbed.h"
00003 #include "mbed_i2c.h"
00004 #include "inv_mpu.h"
00005 #include "inv_mpu_dmp_motion_driver.h"
00006 #include "nrf51.h"
00007 #include "nrf51_bitfields.h"
00008 
00009 #include "BLE.h"
00010 #include "DFUService.h"
00011 #include "UARTService.h"
00012 
00013 #include "W25Q16BV.h"
00014 // flash
00015 DigitalOut vccFlash(p30, 1);
00016 #define PIN_SPI_MOSI p3
00017 #define PIN_SPI_MISO p5
00018 #define PIN_SPI_SCLK p4
00019 #define PIN_CS_FLASH p6
00020 W25Q16BV flash(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCLK, PIN_CS_FLASH); // mosi,miso,clk,cs
00021 int offsetAddr = 0;
00022 bool saveBufferFull = false;
00023 
00024 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00025 
00026 #define LED_GREEN   p21
00027 #define LED_RED     p22
00028 #define LED_BLUE    p23
00029 #define BUTTON_PIN  p17
00030 #define BATTERY_PIN p1
00031 
00032 #define MPU6050_SDA p12
00033 #define MPU6050_SCL p13
00034 
00035 #define UART_TX     p9
00036 #define UART_RX     p11
00037 #define UART_CTS    p8
00038 #define UART_RTS    p10
00039 
00040 /* Starting sampling rate. */
00041 #define DEFAULT_MPU_HZ  (200)
00042 
00043 DigitalOut blue(LED_BLUE);
00044 DigitalOut green(LED_GREEN);
00045 DigitalOut red(LED_RED);
00046 
00047 InterruptIn button(BUTTON_PIN);
00048 AnalogIn    battery(BATTERY_PIN);
00049 Serial pc(UART_TX, UART_RX);
00050 
00051 InterruptIn motion_probe(p14);
00052 
00053 int read_none_count = 0;
00054 
00055 BLEDevice  ble;
00056 UARTService *uartServicePtr;
00057 
00058 volatile bool bleIsConnected = false;
00059 volatile uint8_t tick_event = 0;
00060 volatile uint8_t motion_event = 0;
00061 static signed char board_orientation[9] = {
00062     1, 0, 0,
00063     0, 1, 0,
00064     0, 0, 1
00065 };
00066 
00067 
00068 void check_i2c_bus(void);
00069 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
00070 
00071 
00072 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
00073 {
00074     LOG("Connected!\n");
00075     bleIsConnected = true;
00076 }
00077 
00078 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
00079 {
00080     LOG("Disconnected!\n");
00081     LOG("Restarting the advertising process\n");
00082     ble.startAdvertising();
00083     bleIsConnected = false;
00084 }
00085 
00086 //void tick(void)
00087 //{
00088 //    static uint32_t count = 0;
00089 //    
00090 //    LOG("%d\r\n", count++);
00091 //    green = !green;
00092 //}
00093 
00094 void detect(void)
00095 {
00096     LOG("Button pressed\n");  
00097     blue = !blue;
00098 }
00099 
00100 void motion_interrupt_handle(void)
00101 {
00102     motion_event = 1;
00103 }
00104 
00105 void tap_cb(unsigned char direction, unsigned char count)
00106 {
00107     LOG("Tap motion detected\n");
00108 }
00109 
00110 void android_orient_cb(unsigned char orientation)
00111 {
00112     LOG("Oriention changed\n");
00113 }
00114 
00115 
00116 void saveMPUDataToFlash(int addr, unsigned long timestamp, short accel[3], short gyro[3], long quat[4])
00117 {
00118     uint8_t mpuRawData[32] = {0,};
00119     mpuRawData[0] = timestamp; 
00120     mpuRawData[1] = timestamp>>8;
00121     mpuRawData[2] = timestamp>>16;
00122     mpuRawData[3] = timestamp>>24;
00123     mpuRawData[4] = accel[0]; 
00124     mpuRawData[5] = accel[0]>>8;
00125     mpuRawData[6] = accel[1];
00126     mpuRawData[7] = accel[1]>>8;
00127     mpuRawData[8] = accel[2]; 
00128     mpuRawData[9] = accel[2]>>8;
00129     mpuRawData[10] = gyro[0];
00130     mpuRawData[11] = gyro[0]>>8;
00131     mpuRawData[12] = gyro[1]; 
00132     mpuRawData[13] = gyro[1]>>8;
00133     mpuRawData[14] = gyro[2];
00134     mpuRawData[15] = gyro[2]>>8;
00135     mpuRawData[16] = quat[0]; 
00136     mpuRawData[17] = quat[0]>>8;
00137     mpuRawData[18] = quat[0]>>16;
00138     mpuRawData[19] = quat[0]>>24;
00139     mpuRawData[20] = quat[1]; 
00140     mpuRawData[21] = quat[1]>>8;
00141     mpuRawData[22] = quat[1]>>16;
00142     mpuRawData[23] = quat[1]>>24;
00143     mpuRawData[24] = quat[2]; 
00144     mpuRawData[25] = quat[2]>>8;
00145     mpuRawData[26] = quat[2]>>16;
00146     mpuRawData[27] = quat[2]>>24;
00147     mpuRawData[28] = quat[3]; 
00148     mpuRawData[29] = quat[3]>>8;
00149     mpuRawData[30] = quat[3]>>16;
00150     mpuRawData[31] = quat[3]>>24;
00151     flash.writeStream(addr, (char *)mpuRawData, 32);
00152     pc.printf("%02x %02x %02x %02x\r\n", mpuRawData[0], mpuRawData[1], mpuRawData[2], mpuRawData[3]);
00153 }
00154 
00155 int main(void)
00156 {
00157     blue  = 1;
00158     green = 1;
00159     red   = 1;
00160 
00161     pc.baud(115200);
00162     
00163     pc.printf("SPI init done\n");
00164     flash.chipErase();
00165     pc.printf("Flash Erase done\n");
00166     uint8_t buff[128];
00167     pc.printf("write after erase\r\n");
00168     uint8_t datas[100] = {0,};
00169     for (uint8_t i=0; i<100;i++) {
00170         datas[i] = i+0x01;
00171     }
00172     long dfa = 32524;
00173     datas[0] = dfa;
00174     datas[1] = dfa >> 8;
00175     datas[2] = dfa >> 16;
00176     datas[3] = dfa >> 24;
00177     flash.writeStream(100, (char *)datas, 100); 
00178     
00179     pc.printf("read after write\r\n");
00180     flash.readStream(100, (char*)buff, 100);
00181     for (int i=0; i<100; i++) {
00182         pc.printf("%02x", buff[i]);
00183     }
00184     pc.printf("\r\n");
00185     
00186     pc.printf("%d\r\n", flash.readByte(0x34));
00187     int tmp = 66;
00188     pc.printf("%d\r\n", flash.readByte(tmp));
00189     // read stream from 0x168
00190     char str2[11] = {0};
00191     flash.readStream(tmp, str2, 11);
00192     pc.printf("%s\n",str2);
00193     
00194     for (int i=50; i< 150;i++){
00195         pc.printf("%02x", flash.readByte(i));
00196     }
00197     pc.printf("\r\n");
00198     
00199     char string[] = "ABCDEFGHIJK";
00200     flash.writeStream(tmp, string, 11);
00201     
00202     char str3[11] = {0};
00203     flash.readStream(tmp, str3, 11);
00204     pc.printf("%s\n",str3);
00205     
00206     wait(1);
00207     
00208     LOG("---- Seeed Tiny BLE ----\n");
00209     
00210     mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
00211     mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
00212     
00213 
00214     if (mpu_init(0)) {
00215         LOG("failed to initialize mpu6050\r\n");
00216     }
00217     
00218     mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00219     mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00220     mpu_set_sample_rate(DEFAULT_MPU_HZ);
00221     mpu_set_gyro_fsr(2000);
00222     mpu_set_accel_fsr(16);
00223     dmp_load_motion_driver_firmware();
00224     dmp_set_orientation(
00225         inv_orientation_matrix_to_scalar(board_orientation));
00226     dmp_register_tap_cb(tap_cb);
00227     dmp_register_android_orient_cb(android_orient_cb);
00228     uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
00229                        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | 
00230                        DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
00231     dmp_enable_feature(dmp_features);
00232     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
00233     mpu_set_dmp_state(1);
00234     dmp_set_tap_thresh(TAP_XYZ, 50);
00235     
00236     
00237     motion_probe.fall(motion_interrupt_handle);
00238 
00239     button.fall(detect);
00240 
00241     LOG("Initialising the nRF51822\n");
00242     ble.init();
00243     ble.gap().onDisconnection(disconnectionCallback);
00244     ble.gap().onConnection(connectionCallback);
00245 
00246 
00247     /* setup advertising */
00248     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00249     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00250     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00251                                      (const uint8_t *)"smurfs", sizeof("smurfs"));
00252     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00253                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00254     DFUService dfu(ble);                                 
00255     UARTService uartService(ble);
00256     uartServicePtr = &uartService;
00257     //uartService.retargetStdout();
00258 
00259     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00260     ble.gap().startAdvertising();
00261     
00262     
00263     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};
00264     Timer timer;
00265     timer.start();
00266     while (true) {
00267         if (motion_event) {
00268             
00269             unsigned long sensor_timestamp;
00270             short gyro[3], accel[3], sensors;
00271             long quat[4];
00272             unsigned char more = 1;
00273             
00274             while (more) {
00275                 /* This function gets new data from the FIFO when the DMP is in
00276                  * use. The FIFO can contain any combination of gyro, accel,
00277                  * quaternion, and gesture data. The sensors parameter tells the
00278                  * caller which data fields were actually populated with new data.
00279                  * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
00280                  * the FIFO isn't being filled with accel data.
00281                  * The driver parses the gesture data to determine if a gesture
00282                  * event has occurred; on an event, the application will be notified
00283                  * via a callback (assuming that a callback function was properly
00284                  * registered). The more parameter is non-zero if there are
00285                  * leftover packets in the FIFO.
00286                  */
00287                 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
00288                               &more);
00289                 
00290 //                saveMPUDataToFlash(offsetAddr, sensor_timestamp, accel, gyro, quat);
00291 //                char tmpe[32];
00292 //                flash.readStream(offsetAddr, tmpe, 32);
00293 //                pc.printf("%02x%02x%02x%02x\r\n", tmpe[0],tmpe[1],tmpe[2],tmpe[3]);
00294                 
00295                 /* Gyro and accel data are written to the FIFO by the DMP in chip
00296                  * frame and hardware units. This behavior is convenient because it
00297                  * keeps the gyro and accel outputs of dmp_read_fifo and
00298                  * mpu_read_fifo consistent.
00299                  */
00300                 if (sensors & INV_XYZ_GYRO) {
00301                      LOG("time:%d, GYRO: %d, %d, %d\n", timer.read_ms(), gyro[0], gyro[1], gyro[2]);
00302                 }
00303                 if (sensors & INV_XYZ_ACCEL) {
00304                     //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
00305                 }
00306                 
00307                 /* Unlike gyro and accel, quaternions are written to the FIFO in
00308                  * the body frame, q30. The orientation is set by the scalar passed
00309                  * to dmp_set_orientation during initialization.
00310                  */
00311                 if (sensors & INV_WXYZ_QUAT) {
00312                     // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
00313                 }
00314                 
00315                 if (sensors) {
00316                     read_none_count = 0;
00317                 } else {
00318                     read_none_count++;
00319                     if (read_none_count > 3) {
00320                         read_none_count = 0;
00321                         
00322                         LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
00323                         mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
00324                     }
00325                 }
00326             }
00327             // test flash
00328             flash.writeStream(offsetAddr, (char *)tmp1, 32);
00329             char tmpe[32];
00330             flash.readStream(offsetAddr, tmpe, 32);
00331             for (int i=0; i<32; i++) {
00332                 pc.printf("%02x", tmpe[i]);
00333                 tmp1[i] += 1;
00334             }
00335             pc.printf("\r\n");
00336             offsetAddr += 32;
00337             if (offsetAddr >= 0xFFF) {
00338                 printf("one turn\r\n");
00339                 offsetAddr = 0;
00340             }
00341             motion_event = 0;
00342         } else {
00343             ble.waitForEvent();
00344         }
00345     }
00346 }
00347 
00348 /* These next two functions converts the orientation matrix (see
00349  * gyro_orientation) to a scalar representation for use by the DMP.
00350  * NOTE: These functions are borrowed from Invensense's MPL.
00351  */
00352 static inline unsigned short inv_row_2_scale(const signed char *row)
00353 {
00354     unsigned short b;
00355 
00356     if (row[0] > 0)
00357         b = 0;
00358     else if (row[0] < 0)
00359         b = 4;
00360     else if (row[1] > 0)
00361         b = 1;
00362     else if (row[1] < 0)
00363         b = 5;
00364     else if (row[2] > 0)
00365         b = 2;
00366     else if (row[2] < 0)
00367         b = 6;
00368     else
00369         b = 7;      // error
00370     return b;
00371 }
00372 
00373 unsigned short inv_orientation_matrix_to_scalar(
00374     const signed char *mtx)
00375 {
00376     unsigned short scalar;
00377 
00378     /*
00379        XYZ  010_001_000 Identity Matrix
00380        XZY  001_010_000
00381        YXZ  010_000_001
00382        YZX  000_010_001
00383        ZXY  001_000_010
00384        ZYX  000_001_010
00385      */
00386 
00387     scalar = inv_row_2_scale(mtx);
00388     scalar |= inv_row_2_scale(mtx + 3) << 3;
00389     scalar |= inv_row_2_scale(mtx + 6) << 6;
00390 
00391 
00392     return scalar;
00393 }
00394