Diff: MjHover2.cpp

Revision:

7:a3b72910732d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MjHover2.cpp	Sat Apr 04 14:51:27 2015 +0000
@@ -0,0 +1,104 @@
+#include "MjHover.h"
+
+namespace matsujirushi {
+
+MjHover2::MjHover2(I2C* i2c, uint8_t address, DigitalInOut* ts_n, DigitalInOut* reset_n) : MjHoverBase(i2c, address, ts_n, reset_n)
+{
+}
+
+void MjHover2::Reset()
+{
+    MjHoverBase::Reset();
+    
+    while (!MjHoverBase::DataAvailable())
+    {
+    }
+    
+    uint8_t id;
+    uint8_t payload[255 - 4];
+    uint8_t readPayloadSize;
+    ReadMessage(&id, payload, sizeof (payload), &readPayloadSize);
+}
+
+void MjHover2::Task()
+{
+    uint8_t id;
+    uint8_t payload[255 - 4];
+    uint8_t readPayloadSize;
+    if (MjHoverBase::ReadMessage(&id, payload, sizeof (payload), &readPayloadSize))
+    {
+        if (id == 0x91) // SENSOR_DATA_OUTPUT
+        {
+            this->SensorData.GestureInfoValid = false;
+            this->SensorData.TouchInfoValid   = false;
+            this->SensorData.XyzPositionValid = false;
+            
+            uint16_t dataOutputConfigMask = payload[0] | payload[1] << 8;
+            uint8_t systemInfo = payload[3];
+            uint8_t* data = &payload[4];
+            if (dataOutputConfigMask & 0x0001)  // DSPStatus
+            {
+                data += 2;
+            }
+            if (dataOutputConfigMask & 0x0002)  // GestureInfo
+            {
+                uint32_t gestureInfo = data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
+                uint8_t gesture      =  gestureInfo & 0x0000003f;
+                uint8_t gestureClass = (gestureInfo & 0x0000f000) >> 12;
+                bool edgeFlick       =  gestureInfo & 0x00010000 ? true : false;
+
+                if (gestureClass == 1)  // Flick gesture
+                {
+                    if (gesture == 2)
+                    {
+                        this->SensorData.GestureInfo = edgeFlick ? EDGE_FLICK_WEST_TO_EAST : FLICK_WEST_TO_EAST;
+                        this->SensorData.GestureInfoValid = true;
+                    }
+                    else if (gesture == 3)
+                    {
+                        this->SensorData.GestureInfo = edgeFlick ? EDGE_FLICK_EAST_TO_WEST : FLICK_EAST_TO_WEST;
+                        this->SensorData.GestureInfoValid = true;
+                    }
+                    else if (gesture == 4)
+                    {
+                        this->SensorData.GestureInfo = edgeFlick ? EDGE_FLICK_SOUTH_TO_NORTH : FLICK_SOUTH_TO_NORTH;
+                        this->SensorData.GestureInfoValid = true;
+                    }
+                    else if (gesture == 5)
+                    {
+                        this->SensorData.GestureInfo = edgeFlick ? EDGE_FLICK_NORTH_TO_SOUTH : FLICK_NORTH_TO_SOUTH;
+                        this->SensorData.GestureInfoValid = true;
+                    }
+                }
+                data += 4;
+            }
+            if (dataOutputConfigMask & 0x0004)  // TouchInfo
+            {
+                TouchInfoType touchInfo = (TouchInfoType)((data[0] | data[1] << 8) & 0x7fff);
+                if (touchInfo != 0)
+                {
+                    this->SensorData.TouchInfo = touchInfo;
+                    this->SensorData.TouchInfoValid = true;
+                }
+                data += 4;
+            }
+            if (dataOutputConfigMask & 0x0008)  // AirWheelInfo
+            {
+                data += 2;
+            }
+            if (dataOutputConfigMask & 0x0010)  // xyzPosition
+            {
+                if (systemInfo & 0x01)
+                {
+                    this->SensorData.XyzPosition.X = data[0] | data[1] << 8;
+                    this->SensorData.XyzPosition.Y = data[2] | data[3] << 8;
+                    this->SensorData.XyzPosition.Z = data[4] | data[5] << 8;
+                    this->SensorData.XyzPositionValid = true;
+                }
+                data += 6;
+            }
+        }
+    }
+}
+
+} // namespace matsujirushi