Diff: main.cpp

Revision:

4:19a0764d6b81

Parent:

3:24e365bd1b97

--- a/main.cpp	Thu Nov 05 06:58:30 2015 +0000
+++ b/main.cpp	Fri Nov 13 08:22:27 2015 +0000
@@ -10,6 +10,16 @@
 #include "DFUService.h"
 #include "UARTService.h"
 
+#include "W25Q16BV.h"
+// flash
+DigitalOut vccFlash(p30, 1);
+#define PIN_SPI_MOSI p3
+#define PIN_SPI_MISO p5
+#define PIN_SPI_SCLK p4
+#define PIN_CS_FLASH p6
+W25Q16BV flash(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCLK, PIN_CS_FLASH); // mosi,miso,clk,cs
+int offsetAddr = 0;
+bool saveBufferFull = false;
 
 #define LOG(...)    { pc.printf(__VA_ARGS__); }
 
@@ -28,7 +38,7 @@
 #define UART_RTS    p10
 
 /* Starting sampling rate. */
-#define DEFAULT_MPU_HZ  (100)
+#define DEFAULT_MPU_HZ  (200)
 
 DigitalOut blue(LED_BLUE);
 DigitalOut green(LED_GREEN);
@@ -73,13 +83,13 @@
     bleIsConnected = false;
 }
 
-void tick(void)
-{
-    static uint32_t count = 0;
-    
-    LOG("%d\r\n", count++);
-    green = !green;
-}
+//void tick(void)
+//{
+//    static uint32_t count = 0;
+//    
+//    LOG("%d\r\n", count++);
+//    green = !green;
+//}
 
 void detect(void)
 {
@@ -103,6 +113,45 @@
 }
 
 
+void saveMPUDataToFlash(int addr, unsigned long timestamp, short accel[3], short gyro[3], long quat[4])
+{
+    uint8_t mpuRawData[32] = {0,};
+    mpuRawData[0] = timestamp; 
+    mpuRawData[1] = timestamp>>8;
+    mpuRawData[2] = timestamp>>16;
+    mpuRawData[3] = timestamp>>24;
+    mpuRawData[4] = accel[0]; 
+    mpuRawData[5] = accel[0]>>8;
+    mpuRawData[6] = accel[1];
+    mpuRawData[7] = accel[1]>>8;
+    mpuRawData[8] = accel[2]; 
+    mpuRawData[9] = accel[2]>>8;
+    mpuRawData[10] = gyro[0];
+    mpuRawData[11] = gyro[0]>>8;
+    mpuRawData[12] = gyro[1]; 
+    mpuRawData[13] = gyro[1]>>8;
+    mpuRawData[14] = gyro[2];
+    mpuRawData[15] = gyro[2]>>8;
+    mpuRawData[16] = quat[0]; 
+    mpuRawData[17] = quat[0]>>8;
+    mpuRawData[18] = quat[0]>>16;
+    mpuRawData[19] = quat[0]>>24;
+    mpuRawData[20] = quat[1]; 
+    mpuRawData[21] = quat[1]>>8;
+    mpuRawData[22] = quat[1]>>16;
+    mpuRawData[23] = quat[1]>>24;
+    mpuRawData[24] = quat[2]; 
+    mpuRawData[25] = quat[2]>>8;
+    mpuRawData[26] = quat[2]>>16;
+    mpuRawData[27] = quat[2]>>24;
+    mpuRawData[28] = quat[3]; 
+    mpuRawData[29] = quat[3]>>8;
+    mpuRawData[30] = quat[3]>>16;
+    mpuRawData[31] = quat[3]>>24;
+    flash.writeStream(addr, (char *)mpuRawData, 32);
+    pc.printf("%02x %02x %02x %02x\r\n", mpuRawData[0], mpuRawData[1], mpuRawData[2], mpuRawData[3]);
+}
+
 int main(void)
 {
     blue  = 1;
@@ -111,6 +160,49 @@
 
     pc.baud(115200);
     
+    pc.printf("SPI init done\n");
+    flash.chipErase();
+    pc.printf("Flash Erase done\n");
+    uint8_t buff[128];
+    pc.printf("write after erase\r\n");
+    uint8_t datas[100] = {0,};
+    for (uint8_t i=0; i<100;i++) {
+        datas[i] = i+0x01;
+    }
+    long dfa = 32524;
+    datas[0] = dfa;
+    datas[1] = dfa >> 8;
+    datas[2] = dfa >> 16;
+    datas[3] = dfa >> 24;
+    flash.writeStream(100, (char *)datas, 100); 
+    
+    pc.printf("read after write\r\n");
+    flash.readStream(100, (char*)buff, 100);
+    for (int i=0; i<100; i++) {
+        pc.printf("%02x", buff[i]);
+    }
+    pc.printf("\r\n");
+    
+    pc.printf("%d\r\n", flash.readByte(0x34));
+    int tmp = 66;
+    pc.printf("%d\r\n", flash.readByte(tmp));
+    // read stream from 0x168
+    char str2[11] = {0};
+    flash.readStream(tmp, str2, 11);
+    pc.printf("%s\n",str2);
+    
+    for (int i=50; i< 150;i++){
+        pc.printf("%02x", flash.readByte(i));
+    }
+    pc.printf("\r\n");
+    
+    char string[] = "ABCDEFGHIJK";
+    flash.writeStream(tmp, string, 11);
+    
+    char str3[11] = {0};
+    flash.readStream(tmp, str3, 11);
+    pc.printf("%s\n",str3);
+    
     wait(1);
     
     LOG("---- Seeed Tiny BLE ----\n");
@@ -123,44 +215,27 @@
         LOG("failed to initialize mpu6050\r\n");
     }
     
-    /* Get/set hardware configuration. Start gyro. */
-    /* Wake up all sensors. */
     mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
-    /* Push both gyro and accel data into the FIFO. */
     mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
     mpu_set_sample_rate(DEFAULT_MPU_HZ);
-    
-    /* Read back configuration in case it was set improperly. */
-    unsigned char accel_fsr;
-    unsigned short gyro_rate, gyro_fsr;
-    mpu_get_sample_rate(&gyro_rate);
-    mpu_get_gyro_fsr(&gyro_fsr);
-    mpu_get_accel_fsr(&accel_fsr);
-    
+    mpu_set_gyro_fsr(2000);
+    mpu_set_accel_fsr(16);
     dmp_load_motion_driver_firmware();
     dmp_set_orientation(
         inv_orientation_matrix_to_scalar(board_orientation));
     dmp_register_tap_cb(tap_cb);
     dmp_register_android_orient_cb(android_orient_cb);
-    
     uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
-                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
-                       DMP_FEATURE_GYRO_CAL;
+                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | 
+                       DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
     dmp_enable_feature(dmp_features);
     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
     mpu_set_dmp_state(1);
-    
-    dmp_set_interrupt_mode(DMP_INT_GESTURE);
     dmp_set_tap_thresh(TAP_XYZ, 50);
     
     
     motion_probe.fall(motion_interrupt_handle);
 
-
-    
-    Ticker ticker;
-    ticker.attach(tick, 3);
-
     button.fall(detect);
 
     LOG("Initialising the nRF51822\n");
@@ -184,6 +259,10 @@
     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
     ble.gap().startAdvertising();
     
+    
+    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};
+    Timer timer;
+    timer.start();
     while (true) {
         if (motion_event) {
             
@@ -208,6 +287,10 @@
                 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
                               &more);
                 
+//                saveMPUDataToFlash(offsetAddr, sensor_timestamp, accel, gyro, quat);
+//                char tmpe[32];
+//                flash.readStream(offsetAddr, tmpe, 32);
+//                pc.printf("%02x%02x%02x%02x\r\n", tmpe[0],tmpe[1],tmpe[2],tmpe[3]);
                 
                 /* Gyro and accel data are written to the FIFO by the DMP in chip
                  * frame and hardware units. This behavior is convenient because it
@@ -215,7 +298,7 @@
                  * mpu_read_fifo consistent.
                  */
                 if (sensors & INV_XYZ_GYRO) {
-                    // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
+                     LOG("time:%d, GYRO: %d, %d, %d\n", timer.read_ms(), gyro[0], gyro[1], gyro[2]);
                 }
                 if (sensors & INV_XYZ_ACCEL) {
                     //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
@@ -241,7 +324,20 @@
                     }
                 }
             }
-            
+            // test flash
+            flash.writeStream(offsetAddr, (char *)tmp1, 32);
+            char tmpe[32];
+            flash.readStream(offsetAddr, tmpe, 32);
+            for (int i=0; i<32; i++) {
+                pc.printf("%02x", tmpe[i]);
+                tmp1[i] += 1;
+            }
+            pc.printf("\r\n");
+            offsetAddr += 32;
+            if (offsetAddr >= 0xFFF) {
+                printf("one turn\r\n");
+                offsetAddr = 0;
+            }
             motion_event = 0;
         } else {
             ble.waitForEvent();