Revision 16:799397f0d3a8, committed 2017-05-02

Comitter:

znew711

Date:

Tue May 02 23:42:04 2017 +0000

Parent:

15:d117591084ff

Child:

17:09ceae7cb00e

Commit message:

first draft of thresholding

Changed in this revision

--- a/main.cpp	Tue May 02 04:16:38 2017 +0000
+++ b/main.cpp	Tue May 02 23:42:04 2017 +0000
@@ -85,7 +85,7 @@
 #define DATARATE_50HZ           0b00 // 50Hz
 #define DATARATE_POWERDOWN      0      // Power down
 #define DATARATE_NORMAL_MODE    0b001
-#define DATARATE_LOWPOWER_0.5HZ 0b010
+#define DATARATE_LOWPOWER_05HZ 0b010
 #define DATARATE_LOWPOWER_1HZ   0b011
 #define DATARATE_LOWPOWER_2HZ   0b100
 #define DATARATE_LOWPOWER_5HZ   0b101
@@ -94,7 +94,8 @@
  
 #define PACKET_SIZE 20
 #define QUEUE_SIZE 20
- 
+
+#define Z_THRESHOLD 5000
  
 const static char     DEVICE_NAME[] = "LUMBERJACK_NANO";
 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
@@ -141,6 +142,7 @@
  
 static ButtonService *buttonServicePtr;
 bool isThereAConnection = false;
+bool weAreSending = false;
  
 void sleep(unsigned int mseconds)
 {
@@ -159,6 +161,17 @@
     //pc.printf("Connection recieved!\r\n");
     isThereAConnection = true;
 }
+
+void startTransmission() {
+    uint8_t* nextPacket = removeFromQueue();
+    buttonServicePtr->updateButtonState(nextPacket);
+}
+
+void dataSentCallback(unsigned count) {
+    //pc.printf("dataSent!!\r\n");
+    uint8_t* nextPacket = removeFromQueue();
+    buttonServicePtr->updateButtonState(nextPacket);
+}
  
 /**
  * This function is called when the ble initialization process has failled
@@ -189,6 +202,7 @@
  
     ble.gap().onDisconnection(disconnectionCallback);
     ble.gap().onConnection(connectionCallback);
+    ble.onDataSent(dataSentCallback);
  
     /* Setup primary service */
     uint8_t initial_value[20] = {0, 0, 0, 0, 0,
@@ -254,14 +268,14 @@
     return input;
 }
  
-uint16_t getAxis(uint16_t axis, uint16_t i2cAddr)
+int16_t getAxis(uint16_t axis, uint16_t i2cAddr)
 {
     uint8_t base = REG_OUT_X_L + (2 * axis);
     uint8_t* low = new uint8_t[1];
     uint8_t* high = new uint8_t[1];
     AT24C512_ReadBytes(base, low, 1, i2cAddr);
     AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
-    uint16_t res = low[0] | (high[0] << 8);
+    int16_t res = low[0] | (high[0] << 8);
     delete[] low;
     delete[] high;
     return res;
@@ -353,17 +367,17 @@
     delete[] val;
 }
  
-uint16_t getX(uint16_t i2cAddr)
+int16_t getX(uint16_t i2cAddr)
 {
     return getAxis(AXIS_X, i2cAddr);
 }
  
-uint16_t getY(uint16_t i2cAddr)
+int16_t getY(uint16_t i2cAddr)
 {
     return getAxis(AXIS_Y, i2cAddr);
 }
  
-uint16_t getZ(uint16_t i2cAddr)
+int16_t getZ(uint16_t i2cAddr)
 {
     return getAxis(AXIS_Z, i2cAddr);
 }
@@ -426,31 +440,54 @@
     pq.nextPacketToSend = 0;
     pq.nextSampleToSave = 0;
     pq.liveSamples = 0;
+    
+    uint8_t lastThreePackets[3][20];
+    uint8_t lastThreeIndex = 0;
+    uint8_t underThresholdCount = 0;
+    bool inAKeyStroke = false;
    
     while(1)
     {
         //pc.printf("Read data from AT24C512\r\n");
-        uint16_t x1 = getX(ADDR_ONE);
-        uint16_t y1 = getY(ADDR_ONE);
-        uint16_t z1 = getZ(ADDR_ONE);
+        int16_t x1 = getX(ADDR_ONE);
+        int16_t y1 = getY(ADDR_ONE);
+        int16_t z1 = getZ(ADDR_ONE);
        
-        uint16_t x2 = getX(ADDR_TWO);
-        uint16_t y2 = getY(ADDR_TWO);
-        uint16_t z2 = getZ(ADDR_TWO);
+        int16_t x2 = getX(ADDR_TWO);
+        int16_t y2 = getY(ADDR_TWO);
+        int16_t z2 = getZ(ADDR_TWO);
        
         //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
         //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
-        //pc.printf("\r\n");
-       
-        if(isThereAConnection) {
-            //pc.printf("sending Notification\r\n");
-            uint8_t values[20] = {(uint8_t)x1, (uint8_t)(x1 >> 8), (uint8_t)y1, (uint8_t)(y1 >> 8), (uint8_t)z1, (uint8_t)(z1 >> 8),
-                                  (uint8_t)x2, (uint8_t)(x2 >> 8), (uint8_t)y2, (uint8_t)(y2 >> 8), (uint8_t)z2, (uint8_t)(z2 >> 8),
-                                  0, 0, 0, 0, 0, 0, 0, 0};
-            buttonServicePtr->updateButtonState(values);
+        uint8_t values[20] = {(uint8_t)(x1 >> 8), (uint8_t)x1, (uint8_t)(y1 >> 8), (uint8_t)y1, (uint8_t)(z1 >> 8), (uint8_t)z1,
+                              (uint8_t)(x2 >> 8), (uint8_t)x2, (uint8_t)(y2 >> 8), (uint8_t)y2, (uint8_t)(z2 >> 8), (uint8_t)z2,
+                              0, 0, 0, 0, 0, 0, 0, 0};
+        //TODO: handle negative accels
+        if (z1 > Z_THRESHOLD || z2 > Z_THRESHOLD) {
+            underThresholdCount = 0;
+            if (!inAKeyStroke) {
+                //start transmitting
+                inAKeyStroke = true;
+                for (int i = 0; i < 3; i++) {
+                    addToQueue(lastThreePackets[(lastThreeIndex - 1 - i) % 3]);
+                }
+                addToQueue(values);
+                startTransmission();
+            } else {
+                addToQueue(values);
+            }
+        } else if (underThresholdCount < 3 && inAKeyStroke) {
+            underThresholdCount++;
+            addToQueue(values);
+        } else {
+            for (int i = 0; i < 20; i++) {
+                lastThreePackets[lastThreeIndex][i] = values[i];
+            }
+            lastThreeIndex = (lastThreeIndex + 1) % 3;
+            inAKeyStroke = false;
         }
  
-        wait(1);
+        wait_ms(50);
     }
  
 }
\ No newline at end of file