Diff: glibr.cpp

Revision:

4:84545b0e63a9

Parent:

3:26146a08bb22

Child:

5:3eb4f3091bd8

--- a/glibr.cpp	Thu Mar 05 22:23:50 2015 +0000
+++ b/glibr.cpp	Thu Mar 05 22:42:17 2015 +0000
@@ -374,7 +374,8 @@
     uint8_t val;
     
     /* Read value from GSTATUS register */
-    if( !wireReadDataByte(APDS9960_GSTATUS, val) ) {
+    val=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
+    if( val==ERROR) ) {
         return ERROR;
     }
     
@@ -389,6 +390,118 @@
     }
 }
 
+int glibr::readGesture()
+{
+    uint8_t fifo_level = 0;
+    uint8_t bytes_read = 0;
+    uint8_t fifo_data[128];
+    uint8_t gstatus;
+    int motion;
+    int i;
+    
+    /* Make sure that power and gesture is on and data is valid */
+    if( !isGestureAvailable() || !(getMode() & 0x41) ) {
+        return DIR_NONE;
+    }
+    
+    /* Keep looping as long as gesture data is valid */
+    while(1) {
+    
+        /* Wait some time to collect next batch of FIFO data */
+        delay(FIFO_PAUSE_TIME);
+        
+        /* Get the contents of the STATUS register. Is data still valid? */
+        
+        gstatus=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GSTATUS);
+        if( gstatus==ERROR ) {
+            return ERROR;
+        }
+        /* If we have valid data, read in FIFO */
+        if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
+        
+            /* Read the current FIFO level */
+            fifolevel=I2CreadByte(APDS9960_I2C_ADDR,APDS9960_GFLVL);
+            if( fifolevel==ERROR ) {
+                return ERROR;
+            }
+
+//#if DEBUG
+//            Serial.print("FIFO Level: ");
+//            Serial.println(fifo_level);
+//#endif
+
+            /* If there's stuff in the FIFO, read it into our data block */                 //NEED TO FIGURE OUT WHAT THIS IS DOING.
+            if( fifo_level > 0) {
+                bytes_read = wireReadDataBlock(  APDS9960_GFIFO_U, 
+                                                (uint8_t*)fifo_data, 
+                                                (fifo_level * 4) );
+                if( bytes_read == -1 ) {
+                    return ERROR;
+                }
+#if DEBUG
+                Serial.print("FIFO Dump: ");
+                for ( i = 0; i < bytes_read; i++ ) {
+                    Serial.print(fifo_data[i]);
+                    Serial.print(" ");
+                }
+                Serial.println();
+#endif
+
+                /* If at least 1 set of data, sort the data into U/D/L/R */
+                if( bytes_read >= 4 ) {
+                    for( i = 0; i < bytes_read; i += 4 ) {
+                        gesture_data_.u_data[gesture_data_.index] = \
+                                                            fifo_data[i + 0];
+                        gesture_data_.d_data[gesture_data_.index] = \
+                                                            fifo_data[i + 1];
+                        gesture_data_.l_data[gesture_data_.index] = \
+                                                            fifo_data[i + 2];
+                        gesture_data_.r_data[gesture_data_.index] = \
+                                                            fifo_data[i + 3];
+                        gesture_data_.index++;
+                        gesture_data_.total_gestures++;
+                    }
+                    
+#if DEBUG
+                Serial.print("Up Data: ");
+                for ( i = 0; i < gesture_data_.total_gestures; i++ ) {
+                    Serial.print(gesture_data_.u_data[i]);
+                    Serial.print(" ");
+                }
+                Serial.println();
+#endif
+
+                    /* Filter and process gesture data. Decode near/far state */
+                    if( processGestureData() ) {
+                        if( decodeGesture() ) {
+                            //***TODO: U-Turn Gestures
+#if DEBUG
+                            //Serial.println(gesture_motion_);
+#endif
+                        }
+                    }
+                    
+                    /* Reset data */
+                    gesture_data_.index = 0;
+                    gesture_data_.total_gestures = 0;
+                }
+            }
+        } else {
+    
+            /* Determine best guessed gesture and clean up */
+            delay(FIFO_PAUSE_TIME);
+            decodeGesture();
+            motion = gesture_motion_;
+#if DEBUG
+            Serial.print("END: ");
+            Serial.println(gesture_motion_);
+#endif
+            resetGestureParameters();
+            return motion;
+        }
+    }
+}
+
 bool glibr::setProximityGain(uint8_t drive)
 {
     uint8_t val;