From ff974b6cd5a7bc1030a3db9fbbfc3955c5f9e34c Mon Sep 17 00:00:00 2001
From: Stephen Birarda <commit@birarda.com>
Date: Wed, 1 May 2013 12:50:02 -0700
Subject: [PATCH] half baked communication with shitty invensense

---
 CMakeLists.txt                 |  3 +-
 invensense-test/CMakeLists.txt | 13 ++++++
 invensense-test/src/main.cpp   | 79 ++++++++++++++++++++++++++++++++++
 3 files changed, 94 insertions(+), 1 deletion(-)
 create mode 100644 invensense-test/CMakeLists.txt
 create mode 100644 invensense-test/src/main.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3923f543fa..5b1001119d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,4 +9,5 @@ add_subdirectory(eve)
 add_subdirectory(interface)
 add_subdirectory(injector)
 add_subdirectory(space-server)
-add_subdirectory(voxel-server)
\ No newline at end of file
+add_subdirectory(voxel-server)
+add_subdirectory(invensense-test)
\ No newline at end of file
diff --git a/invensense-test/CMakeLists.txt b/invensense-test/CMakeLists.txt
new file mode 100644
index 0000000000..b0dfed8990
--- /dev/null
+++ b/invensense-test/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8)
+
+set(ROOT_DIR ..)
+set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
+
+set(TARGET_NAME invensense-test)
+
+include(${MACRO_DIR}/SetupHifiProject.cmake)
+setup_hifi_project(${TARGET_NAME})
+
+# link the shared hifi library
+include(${MACRO_DIR}/LinkHifiLibrary.cmake)
+link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
\ No newline at end of file
diff --git a/invensense-test/src/main.cpp b/invensense-test/src/main.cpp
new file mode 100644
index 0000000000..981c8508f0
--- /dev/null
+++ b/invensense-test/src/main.cpp
@@ -0,0 +1,79 @@
+//
+//  main.cpp
+//  invensense-test
+//
+//  Created by Stephen Birarda on 4/30/13.
+//  Copyright (c) 2013 High Fidelity. All rights reserved.
+//
+
+#include <iostream>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <dirent.h>
+#include <SharedUtil.h>
+
+int main(int argc, char* argv[]) {
+    int serialFd = open("/dev/tty.usbmodemfa141", O_RDWR | O_NOCTTY | O_NDELAY);
+    
+    struct termios options;
+    tcgetattr(serialFd, &options);
+    
+    cfsetispeed(&options,B115200);
+    cfsetospeed(&options,B115200);
+    
+    options.c_cflag |= (CLOCAL | CREAD);
+    options.c_cflag &= ~PARENB;
+    options.c_cflag &= ~CSTOPB;
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8;
+    tcsetattr(serialFd,TCSANOW,&options);
+    
+    // Flush the port's buffers (in and out) before we start using it
+    tcflush(serialFd, TCIOFLUSH);
+    
+    // block on read until data is available
+    int currentFlags = fcntl(serialFd, F_GETFL);    
+    fcntl(serialFd, F_SETFL, currentFlags & ~O_NONBLOCK);
+    
+    write(serialFd, "WR686B01\n", 9);
+    usleep(100000);
+    
+    tcflush(serialFd, TCIOFLUSH);
+    
+    char gyroBuffer[12] = {};
+    int16_t gyroX, gyroY, gyroZ;
+    
+    char statusByte[4] = {};
+    int dataReadyVal = 0;
+    
+    while (true) {
+        write(serialFd, "RS3A\n", 5);
+        read(serialFd, statusByte, 4);
+        
+        printf("The status byte read was %c%c%c%c\n", statusByte[0], statusByte[1], statusByte[2], statusByte[3]);
+        dataReadyVal = statusByte[3] - '0';
+        
+        if (dataReadyVal % 2 == 1) {
+            tcflush(serialFd, TCIOFLUSH);
+            
+            write(serialFd, "RD684306\n", 9);
+            printf("Read %ld bytes from serial\n", read(serialFd, gyroBuffer, 12));
+            printf("The first six chars %c%c%c%c%c%c\n", gyroBuffer[0], gyroBuffer[1], gyroBuffer[2], gyroBuffer[3], gyroBuffer[4], gyroBuffer[5]);
+
+            gyroX = (gyroBuffer[6] << 8) | gyroBuffer[7];
+            gyroY = (gyroBuffer[8] << 8) | gyroBuffer[9];
+            gyroZ = (gyroBuffer[10] << 8) | gyroBuffer[11];
+            
+            printf("The gyro data %d, %d, %d\n", gyroX, gyroY, gyroZ);
+        }
+        
+        tcflush(serialFd, TCIOFLUSH);
+    }
+    
+    close(serialFd);
+    
+    return 0;
+}
+
+