Merge remote-tracking branch 'upstream/master'

interface/src/Application.cpp

@@ -114,7 +114,7 @@ void GLCanvas::wheelEvent(QWheelEvent* event) {
     Application::getInstance()->wheelEvent(event);
 }
 
-Application::Application(int& argc, char** argv) :
+Application::Application(int& argc, char** argv, timeval &startup_time) :
         QApplication(argc, argv),
         _window(new QMainWindow(desktop())),
         _glWidget(new GLCanvas()),
@@ -152,7 +152,7 @@ Application::Application(int& argc, char** argv) :
         _bytesPerSecond(0),
         _bytesCount(0)
 {
-    gettimeofday(&_applicationStartupTime, NULL);
+    _applicationStartupTime = startup_time;
     _window->setWindowTitle("Interface");
     printLog("Interface Startup:\n");
     
@@ -268,6 +268,15 @@ void Application::initializeGL() {
     QTimer* idleTimer = new QTimer(this);
     connect(idleTimer, SIGNAL(timeout()), SLOT(idle()));
     idleTimer->start(0);
+    
+    if (_justStarted) {
+        float startupTime = (usecTimestampNow() - usecTimestamp(&_applicationStartupTime))/1000000.0;
+        _justStarted = false;
+        char title[50];
+        sprintf(title, "Interface: %4.2f seconds\n", startupTime);
+        printLog("%s", title);
+        _window->setWindowTitle(title);
+    }
 }
 
 void Application::paintGL() {
@@ -403,16 +412,6 @@ void Application::paintGL() {
     
     
     _frameCount++;
-        
-    //  If application has just started, report time from startup to now (first frame display)
-    if (_justStarted) {
-        float startupTime = (usecTimestampNow() - usecTimestamp(&_applicationStartupTime))/1000000.0;
-        _justStarted = false;
-        char title[50];
-        sprintf(title, "Interface: %4.2f seconds\n", startupTime);
-        printLog("%s", title);
-        _window->setWindowTitle(title);
-    }
 }
 
 void Application::resizeGL(int width, int height) {
@@ -1193,8 +1192,10 @@ void Application::initMenu() {
     _window->setMenuBar(menuBar);
     
     QMenu* fileMenu = menuBar->addMenu("File");
-    fileMenu->addAction("Pair", this, SLOT(pair()));
     fileMenu->addAction("Quit", this, SLOT(quit()), Qt::Key_Q);
+
+    QMenu* pairMenu = menuBar->addMenu("Pair");
+    pairMenu->addAction("Pair", this, SLOT(pair()));
     
     QMenu* optionsMenu = menuBar->addMenu("Options");
     (_lookingInMirror = optionsMenu->addAction("Mirror", this, SLOT(setHead(bool)), Qt::Key_H))->setCheckable(true);

interface/src/Application.h

@@ -46,7 +46,7 @@ class Application : public QApplication {
 public:
     static Application* getInstance() { return static_cast<Application*>(QCoreApplication::instance()); }
 
-    Application(int& argc, char** argv);
+    Application(int& argc, char** argv, timeval &startup_time);
 
     void initializeGL();
     void paintGL();

interface/src/Avatar.cpp

@@ -279,18 +279,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_FWD_FORCE_SCALE = 100.f;
-            const float TRANSMITTER_YAW_SCALE = 7.0f;
+            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

@@ -65,13 +65,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);
+        _pitch *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime);
-        _yaw   *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime);
+        _yaw   *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime);
-        _roll  *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * 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) +
+    const int PACKET_HEADER_SIZE = 1;                   //  Packet's first byte is 'T'
-        sizeof(_lastAcceleration)) {
+    const int ROTATION_MARKER_SIZE = 1;                 //  'R' = Rotation (clockwise about x,y,z)
-        memcpy(&_lastRotationRate, packetData + 2, sizeof(_lastRotationRate));
+    const int ACCELERATION_MARKER_SIZE = 1;             //  'A' = Acceleration (x,y,z)
-        memcpy(&_lastAcceleration, packetData + 3 + sizeof(_lastAcceleration), sizeof(_lastAcceleration));
+    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

@@ -14,6 +14,12 @@
 #include <glm/gtc/quaternion.hpp>
 #include <cstring>
 #include "world.h"
+#include <stdint.h> 
+
+struct TouchState {
+    uint16_t x, y;
+    char state;
+};
 
 class Transmitter
 {
@@ -26,6 +32,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 +40,7 @@ private:
     glm::vec3 _lastRotationRate;
     glm::vec3 _lastAcceleration;
     glm::vec3 _estimatedRotation;
+    TouchState _touchState;
 
 #endif /* defined(__hifi__Transmitter__) */
 };

interface/src/main.cpp

@@ -1,28 +1,31 @@
-//  
+//  
-//  Interface
+//  Interface
-//
+//
-//  Allows you to connect to and see/hear the shared 3D space.
+//  Allows you to connect to and see/hear the shared 3D space.
-//  Optionally uses serialUSB connection to get gyro data for head movement.
+//  Optionally uses serialUSB connection to get gyro data for head movement.
-//  Optionally gets UDP stream from transmitter to animate controller/hand.
+//  Optionally gets UDP stream from transmitter to animate controller/hand.
-//  
+//  
-//  Usage:  The interface client first attempts to contact a domain server to
+//  Usage:  The interface client first attempts to contact a domain server to
-//          discover the appropriate audio, voxel, and avatar servers to contact.
+//          discover the appropriate audio, voxel, and avatar servers to contact.
-//          Right now, the default domain server is "highfidelity.below92.com"
+//          Right now, the default domain server is "highfidelity.below92.com"
-//          You can change the domain server to use your own by editing the
+//          You can change the domain server to use your own by editing the
-//          DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp
+//          DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp
-//
+//
-//
+//
-//  Welcome Aboard!
+//  Welcome Aboard!
-//
+//
-
+
-#include "Application.h"
+#include "Application.h"
-#include "Log.h"
+#include "Log.h"
-
+
-int main(int argc, const char * argv[]) {
+int main(int argc, const char * argv[]) {
-    
+    
-    Application app(argc, const_cast<char**>(argv));
+    timeval startup_time;
-    printLog( "Created QT Application.\n" );
+    gettimeofday(&startup_time, NULL);
-    int exitCode = app.exec();
+    
-    printLog("Normal exit.\n");
+    Application app(argc, const_cast<char**>(argv), startup_time);
-    return exitCode;
+    printLog( "Created QT Application.\n" );
-}   
+    int exitCode = app.exec();
+    printLog("Normal exit.\n");
+    return exitCode;
+}