int apds9960ReadSensor(void)
{
int Gesture = 0;
if(isGestureAvailable())
{
Gesture = readGesture();
}
return Gesture;
}
int 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() & 0b01000001) ) {
return DIR_NONE;
}
/* Keep looping as long as gesture data is valid */
while(1) {
/* Wait some time to collect next batch of FIFO data */
//sleep(2000);
/* Get the contents of the STATUS register. Is data still valid? */
wireReadDataByte(APDS9960_GSTATUS, &gstatus);
/* If we have valid data, read in FIFO */
if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
/* Read the current FIFO level */
wireReadDataByte(APDS9960_GFLVL, &fifo_level);
#if DEBUG
Serial.print("FIFO Level: ");
Serial.println(fifo_level);
#endif
/* If there's stuff in the FIFO, read it into our data block */
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;
}
}
}
