Revision 4:40d23f4295f9, committed 2021-02-10

Comitter:

TVA

Date:

Wed Feb 10 19:45:43 2021 +0000

Parent:

3:0e37515b92f1

Commit message:

Working test

Changed in this revision

diff -r 0e37515b92f1 -r 40d23f4295f9 main.cpp
--- a/main.cpp	Mon Feb 08 20:59:53 2021 +0000
+++ b/main.cpp	Wed Feb 10 19:45:43 2021 +0000
@@ -6,6 +6,29 @@
 #include "mbed.h"
 #include "platform/mbed_thread.h"
 
+// populate from databuffer queue raw samples data before sending over ble
+// struct RawData samples_raw_data;
+// int sampleIndex = 0;
+// int ptr = 0;
+// while(sampleIndex < mumberOffSamples)
+//  status byte is index 0
+//  samples_raw_data.green_led_cnt  = (databuf[ptr+1]  << 16)  + (databuf[ptr+2]  << 8)  + (databuf[ptr+3]);
+//  samples_raw_data.ir_led_cnt     = (databuf[ptr+4]  << 16)  + (databuf[ptr+5]  << 8)  + (databuf[ptr+6]);
+//  samples_raw_data.red_led_cnt    = (databuf[ptr+7]  << 16)  + (databuf[ptr+8]  << 8)  + (databuf[ptr+9]);
+//  samples_raw_data.green2_led_cnt = (databuf[ptr+10] << 16)  + (databuf[ptr+11] << 8)  + (databuf[ptr12]);
+//  samples_raw_data.x              = (databuf[ptr+19] << 8)   + (databuf[ptr+20]);
+//  samples_raw_data.y              = (databuf[ptr+21] << 8)   + (databuf[ptr+22]);
+//  samples_raw_data.z              = (databuf[ptr+23] << 8)   + (databuf[ptr+24]);
+struct RawData
+{
+  uint32_t green_led_cnt;
+  uint32_t ir_led_cnt;
+  uint32_t red_led_cnt;
+  uint32_t green2_led_cnt;
+  int16_t x;
+  int16_t y;
+  int16_t z;
+};
 
 // Blinking rate in milliseconds
 #define BLINKING_RATE_MS                                                    500
@@ -39,6 +62,7 @@
     //scanI2C();
 
     while (true) {
+        //Read out status (app or boot mode)
         cmd[0] = 0x02;
         cmd[1] = 0x00;
         i2c.write(addr, cmd, 2);
@@ -48,27 +72,19 @@
         cmd[1] = 0x00;
         i2c.read(addr, cmd, 2);
         printf("1: %x %x\n", cmd[0], cmd[1]);
-        
-        
-        
-        
-        
-        
+    
+        //Switch to APP mode
         cmd[0] = 0x01;
         cmd[1] = 0x00;
         cmd[2] = 0x00;
         i2c.write(addr, cmd, 3);
         thread_sleep_for(1500);
         
-        
-        
+        //Switch off MFIO
         mfio = 0;
-        thread_sleep_for(1);
+        thread_sleep_for(1); //Wait 300us
         
-        
-        
-        
-        
+        //Read out status (app or boot mode)
         cmd[0] = 0x02;
         cmd[1] = 0x00;
         i2c.write(addr, cmd, 2);
@@ -78,11 +94,8 @@
         cmd[1] = 0x00;
         i2c.read(addr, cmd, 2);
         printf("2: %x %x\n", cmd[0], cmd[1]);
-        
-        
-        
-        
 
+        //Sensor hub firmware nummer opvragen
         cmd[0] = 0xFF;
         cmd[1] = 0x03;
         i2c.write(addr, cmd, 2);
@@ -94,10 +107,8 @@
         cmd[3] = 0x00;
         i2c.read(addr, cmd, 4);
         printf("3: %d %d %d %d\n", cmd[0], cmd[1], cmd[2], cmd[3]);
-        
-        
-        
-        
+
+        //Raw data mode
         cmd[0] = 0x10;
         cmd[1] = 0x00;
         cmd[2] = 0x03;
@@ -106,15 +117,10 @@
         thread_sleep_for(2);
         
         cmd[0] = 0x00;
-        cmd[1] = 0x00;
-        i2c.read(addr, cmd, 2);
-        printf("4: %d %d\n", cmd[0] , cmd[1]);
-        
+        i2c.read(addr, cmd, 1);
+        printf("4: %x\n", cmd[0]);
         
-        
-        
-        
-        
+        //Interrupt threshold
         cmd[0] = 0x10;
         cmd[1] = 0x01;
         cmd[2] = 0x01;
@@ -123,15 +129,10 @@
         thread_sleep_for(2);
         
         cmd[0] = 0x00;
-        cmd[1] = 0x00;
-        i2c.read(addr, cmd, 2);
-        printf("5: %d %d\n", cmd[0], cmd[1]);
-        
+        i2c.read(addr, cmd, 1);
+        printf("5: %x\n", cmd[0]);
         
-        
-        
-        
-        
+        //Set sample report period
         cmd[0] = 0x10;
         cmd[1] = 0x02;
         cmd[2] = 0x01;
@@ -140,32 +141,23 @@
         thread_sleep_for(2);
         
         cmd[0] = 0x00;
-        cmd[1] = 0x00;
-        i2c.read(addr, cmd, 2);
-        printf("6: %d %d\n", cmd[0], cmd[1]);
-        
+        i2c.read(addr, cmd, 1);
+        printf("6: %x\n", cmd[0]);
         
-        
-        
-        
-        
+        //Enable accelerometer
         cmd[0] = 0x44;
         cmd[1] = 0x04;
         cmd[2] = 0x01;
-        cmd[3] = 0x01;
+        cmd[3] = 0x00; //0x01
         
         i2c.write(addr, cmd, 4);
         thread_sleep_for(20);
         
         cmd[0] = 0x00;
-        cmd[1] = 0x00;
-        i2c.read(addr, cmd, 2);
-        printf("7: %d %d\n", cmd[0], cmd[1]);
+        i2c.read(addr, cmd, 1);
+        printf("7: %x\n", cmd[0]);
         
-        
-        
-        
-        
+        //Read accelerometer WHO_AM_I register
         cmd[0] = 0x41;
         cmd[1] = 0x04;
         cmd[2] = 0x0F;   
@@ -176,12 +168,129 @@
         cmd[0] = 0x00;
         cmd[1] = 0x00;
         i2c.read(addr, cmd, 2);
-        printf("8: %d %d\n", cmd[0], cmd[1]);
+        printf("8: %x %x\n", cmd[0], cmd[1]);
+        
+        
+        
+        printf("Start\n");
+
+
+
+        //Enable AFE
+        cmd[0] = 0x44;
+        cmd[1] = 0x00;
+        cmd[2] = 0x01; 
+        //MAYBE 4
+        
+        i2c.write(addr, cmd, 3);
+        thread_sleep_for(250);
+        
+        cmd[0] = 0x00;
+        i2c.read(addr, cmd, 1);
+        printf("9: %x\n", cmd[0]);
+        
+        //Set 100Hz read samples
+        cmd[0] = 0x40;
+        cmd[1] = 0x00;
+        cmd[2] = 0x12; 
+        cmd[3] = 0x18; 
+                
+        i2c.write(addr, cmd, 4);
+        thread_sleep_for(2);
+        
+        cmd[0] = 0x00;
+        i2c.read(addr, cmd, 1);
+        printf("10: %x\n", cmd[0]);
+        
+        
+        
+        
+        //get data
+        struct RawData samples_raw_data;
+        int sampleIndex = 0;
+        char rawData[1024];
+        uint8_t amount = 0;
+        memset(rawData,0,sizeof(rawData));
+        for(uint8_t i=0; i<250; i++)
+        {
+            //Get status sensor hub
+            cmd[0] = 0x00;
+            cmd[1] = 0x00;
+            i2c.write(addr, cmd, 2);
+            thread_sleep_for(2);
+        
+            cmd[0] = 0x00;
+            cmd[1] = 0x00;
+            i2c.read(addr, cmd, 2);
+            printf("dat1: %x %x\n", cmd[0], cmd[1]);
+            
+            //Get number of samples in the fifo
+            cmd[0] = 0x12;
+            cmd[1] = 0x00;
+            i2c.write(addr, cmd, 2);
+            thread_sleep_for(2);
+        
+            cmd[0] = 0x00;
+            cmd[1] = 0x00;
+            i2c.read(addr, cmd, 2);
+            amount = cmd[1];
+            printf("dat2: %x %x\n", cmd[0], cmd[1]);
+            
+            //Get data from fifo
+            cmd[0] = 0x12;
+            cmd[1] = 0x01;
+            i2c.write(addr, cmd, 2);
+            thread_sleep_for(2);
+            
+            i2c.read(addr, rawData, 1+((36+6)*amount));
+            
+            printf("Satus: %x\n", rawData[0]);
+            printf("Data: ");
+            for(uint32_t j=1; j<1+((36+6)*amount); j+=42)
+            {
+                //%zu
+                printf("%u, %d, %d, %d\n", (uint32_t)(0x00<<24 | rawData[j+1]<<16 | rawData[j+2]<<8 | rawData[j+3]), (rawData[j+37]<<8 | rawData[j+38]), (rawData[j+39]<<8 | rawData[j+40]), (rawData[j+41]<<8 | rawData[j+42]));
+            }
+                
+            //thread_sleep_for(200); 
+            thread_sleep_for(1);       
+        }
         
         
         
         
         
+        //Disable AFE
+        cmd[0] = 0x44;
+        cmd[1] = 0x00;
+        cmd[2] = 0x00; 
+        //MAYBE 4
+        
+        i2c.write(addr, cmd, 3);
+        thread_sleep_for(250);
+        
+        cmd[0] = 0x00;
+        i2c.read(addr, cmd, 1);
+        printf("11: %x\n", cmd[0]);
+        
+        
+        
+        printf("Stop\n");
+        
+        
+        
+        //Disable Accelerometer
+        cmd[0] = 0x44;
+        cmd[1] = 0x04;
+        cmd[2] = 0x00; 
+        //MAYBE 4
+        
+        i2c.write(addr, cmd, 3);
+        thread_sleep_for(20);
+        
+        cmd[0] = 0x00;
+        i2c.read(addr, cmd, 1);
+        printf("12: %x\n", cmd[0]);
 
         /*rst =! rst;
         mfio =! mfio;*/