Merge pull request #379 from PhilipRosedale/master

Sensor fusion to better stabilize transmitter roll, pitch

interface/src/Avatar.cpp

@@ -278,18 +278,29 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
         if (transmitter) {
             glm::vec3 rotation = transmitter->getEstimatedRotation();
             const float TRANSMITTER_MIN_RATE = 1.f;
+            const float TRANSMITTER_MIN_YAW_RATE = 4.f;
             const float TRANSMITTER_LATERAL_FORCE_SCALE = 25.f;
-            const float TRANSMITTER_FWD_FORCE_SCALE = 50.f;
-            const float TRANSMITTER_YAW_SCALE = 7.0f;
+            const float TRANSMITTER_FWD_FORCE_SCALE = 25.f;
+            const float TRANSMITTER_YAW_SCALE = 10.0f;
+            const float TRANSMITTER_LIFT_SCALE = 3.f;
+            const float TOUCH_POSITION_RANGE_HALF = 32767.f;
             if (fabs(rotation.z) > TRANSMITTER_MIN_RATE) {
                 _thrust += rotation.z * TRANSMITTER_LATERAL_FORCE_SCALE * deltaTime * _orientation.getRight();
             }
             if (fabs(rotation.x) > TRANSMITTER_MIN_RATE) {
                 _thrust += -rotation.x * TRANSMITTER_FWD_FORCE_SCALE * deltaTime * _orientation.getFront();
             }
-            if (fabs(rotation.y) > TRANSMITTER_MIN_RATE) {
+            if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) {
                 _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime;
             }
+            if (transmitter->getTouchState()->state == 'D') {
+                _thrust += THRUST_MAG *
+                           (float)(transmitter->getTouchState()->y - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF *
+                           TRANSMITTER_LIFT_SCALE *
+                           deltaTime *
+                           _orientation.getUp();
+            }
+            
         }
 	}
         

interface/src/Head.cpp

@@ -18,7 +18,6 @@ const float EYE_FRONT_OFFSET        =  0.8f;
 const float EAR_RIGHT_OFFSET        =  1.0;
 const float MOUTH_FRONT_OFFSET      =  1.0f;
 const float MOUTH_UP_OFFSET         = -0.3f;
-const float HEAD_MOTION_DECAY       = 0.1;
 const float MINIMUM_EYE_ROTATION    = 0.7f; // based on a dot product: 1.0 is straight ahead, 0.0 is 90 degrees off
 const float EYEBALL_RADIUS          = 0.02; 
 const float EYEBALL_COLOR[3]        = { 0.9f, 0.9f, 0.8f };
@@ -56,13 +55,15 @@ void Head::reset() {
 
 void Head::simulate(float deltaTime, bool isMine) {
 
+    const float HEAD_MOTION_DECAY = 0.00;
+    
     //  Decay head back to center if turned on
     if (isMine && _returnHeadToCenter) {
     
         //  Decay rotation back toward center
-        _pitch *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime);
-        _yaw   *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime);
-        _roll  *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime);
+        _pitch *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime);
+        _yaw   *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime);
+        _roll  *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime);
     }
     
     //  For invensense gyro, decay only slightly when roughly centered

interface/src/Transmitter.cpp

@@ -11,6 +11,7 @@
 #include "Util.h"
 #include <cstring>
 #include <glm/glm.hpp>
+#include "Log.h"
 
 const float DELTA_TIME = 1.f / 60.f;
 const float DECAY_RATE = 0.15f;
@@ -30,24 +31,50 @@ void Transmitter::resetLevels() {
 }
 
 void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) {
-    if (numBytes == 3 + sizeof(_lastRotationRate) +
-        sizeof(_lastAcceleration)) {
-        memcpy(&_lastRotationRate, packetData + 2, sizeof(_lastRotationRate));
-        memcpy(&_lastAcceleration, packetData + 3 + sizeof(_lastAcceleration), sizeof(_lastAcceleration));
-       
+    const int PACKET_HEADER_SIZE = 1;                   //  Packet's first byte is 'T'
+    const int ROTATION_MARKER_SIZE = 1;                 //  'R' = Rotation (clockwise about x,y,z)
+    const int ACCELERATION_MARKER_SIZE = 1;             //  'A' = Acceleration (x,y,z)
+    if (numBytes == PACKET_HEADER_SIZE + ROTATION_MARKER_SIZE + ACCELERATION_MARKER_SIZE
+            + sizeof(_lastRotationRate) + sizeof(_lastAcceleration)
+            + sizeof(_touchState.x) + sizeof(_touchState.y) + sizeof(_touchState.state)) {
+        unsigned char* packetDataPosition = &packetData[PACKET_HEADER_SIZE + ROTATION_MARKER_SIZE];
+        memcpy(&_lastRotationRate, packetDataPosition, sizeof(_lastRotationRate));
+        packetDataPosition += sizeof(_lastRotationRate) + ACCELERATION_MARKER_SIZE;
+        memcpy(&_lastAcceleration, packetDataPosition, sizeof(_lastAcceleration));
+        packetDataPosition += sizeof(_lastAcceleration);
+        memcpy(&_touchState.state, packetDataPosition, sizeof(_touchState.state));
+        packetDataPosition += sizeof(_touchState.state);
+        memcpy(&_touchState.x, packetDataPosition, sizeof(_touchState.x));
+        packetDataPosition += sizeof(_touchState.x);
+        memcpy(&_touchState.y, packetDataPosition, sizeof(_touchState.y));
+        packetDataPosition += sizeof(_touchState.y);
+
         //  Update estimated absolute position from rotation rates
         _estimatedRotation += _lastRotationRate * DELTA_TIME;
-        
-        //  Decay estimated absolute position to slowly return to zero regardless
-        _estimatedRotation *= (1.f - DECAY_RATE * DELTA_TIME); 
     
+        // Sensor Fusion!  Slowly adjust estimated rotation to be relative to gravity (average acceleration)
+        const float GRAVITY_FOLLOW_RATE = 1.f;
+        float rollAngle = angleBetween(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0.f), glm::vec3(0,-1,0)) *
+                          ((_lastAcceleration.x < 0.f) ? -1.f : 1.f);
+        float pitchAngle = angleBetween(glm::vec3(0.f, _lastAcceleration.y, _lastAcceleration.z), glm::vec3(0,-1,0)) *
+                          ((_lastAcceleration.z < 0.f) ? 1.f : -1.f);
+
+        _estimatedRotation.x = (1.f - GRAVITY_FOLLOW_RATE * DELTA_TIME) * _estimatedRotation.x +
+                                GRAVITY_FOLLOW_RATE * DELTA_TIME * pitchAngle;
+        _estimatedRotation.z = (1.f - GRAVITY_FOLLOW_RATE * DELTA_TIME) * _estimatedRotation.z +
+                                GRAVITY_FOLLOW_RATE * DELTA_TIME * rollAngle;
+        
+        //  Can't apply gravity fusion to Yaw, so decay estimated yaw to zero,
+        //  presuming that the average yaw direction is toward screen
+        _estimatedRotation.y *= (1.f - DECAY_RATE * DELTA_TIME);
+
         if (!_isConnected) {
             printf("Transmitter V2 Connected.\n");
             _isConnected = true;
             _estimatedRotation *= 0.0;
         }
     } else {
-        printf("Transmitter V2 packet read error.\n");
+        printf("Transmitter V2 packet read error, %d bytes.\n", numBytes);
     }
 }
 

interface/src/Transmitter.h

@@ -15,6 +15,11 @@
 #include <cstring>
 #include "world.h"
 
+struct TouchState {
+    uint16_t x, y;
+    char state;
+};
+
 class Transmitter
 {
 public:
@@ -26,6 +31,7 @@ public:
     const glm::vec3 getLastRotationRate() const { return _lastRotationRate; };
     const glm::vec3 getLastAcceleration() const { return _lastRotationRate; };
     const glm::vec3 getEstimatedRotation() const { return _estimatedRotation; };
+    const TouchState* getTouchState() const { return &_touchState; };
     void processIncomingData(unsigned char* packetData, int numBytes);
 
 private:
@@ -33,6 +39,7 @@ private:
     glm::vec3 _lastRotationRate;
     glm::vec3 _lastAcceleration;
     glm::vec3 _estimatedRotation;
+    TouchState _touchState;
 
 #endif /* defined(__hifi__Transmitter__) */
 };