From 6ff4b88ea4da0da9b501a9b364f9856cdfc01d84 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 21 May 2013 15:46:11 -0700 Subject: [PATCH 1/5] starting tweaks to head gyro motion --- interface/src/Head.cpp | 9 +++++---- interface/src/Transmitter.cpp | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index b5afb1bd01..de14e7bf43 100644 --- a/interface/src/Head.cpp +++ b/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.2f; -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 diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp index 917c98847f..a03e767371 100644 --- a/interface/src/Transmitter.cpp +++ b/interface/src/Transmitter.cpp @@ -30,7 +30,7 @@ void Transmitter::resetLevels() { } void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) { - if (numBytes == 3 + sizeof(_lastRotationRate) + + if (numBytes >= 3 + sizeof(_lastRotationRate) + sizeof(_lastAcceleration)) { memcpy(&_lastRotationRate, packetData + 2, sizeof(_lastRotationRate)); memcpy(&_lastAcceleration, packetData + 3 + sizeof(_lastAcceleration), sizeof(_lastAcceleration)); From 2edd08594114b87f38e2f2a828029e61a064c3d2 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 21 May 2013 22:40:13 -0700 Subject: [PATCH 2/5] Added sensor fusion with gravity to transmitter to stabilize drift offset in pitch and roll. --- interface/src/Avatar.cpp | 7 +++--- interface/src/Transmitter.cpp | 40 ++++++++++++++++++++++++++++------- interface/src/Transmitter.h | 2 ++ 3 files changed, 38 insertions(+), 11 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 9ca648a44a..7728b0bdfe 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -278,16 +278,17 @@ 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; 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; } } diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp index a03e767371..3a3a3f3ff8 100644 --- a/interface/src/Transmitter.cpp +++ b/interface/src/Transmitter.cpp @@ -11,6 +11,7 @@ #include "Util.h" #include #include +#include "Log.h" const float DELTA_TIME = 1.f / 60.f; const float DECAY_RATE = 0.15f; @@ -31,23 +32,46 @@ 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)); - + sizeof(_lastAcceleration) + sizeof(_touchState)) { + unsigned char* ptr = &packetData[2]; + memcpy(&_lastRotationRate, ptr, sizeof(_lastRotationRate)); + ptr += sizeof(_lastRotationRate) + 1; + memcpy(&_lastAcceleration, ptr, sizeof(_lastAcceleration)); + ptr += sizeof(_lastAcceleration); + memcpy(&_touchState, ptr, sizeof(_touchState)); + ptr += sizeof(_touchState); + if (_touchState == 'D') { + memcpy(&_touchPoint, ptr, sizeof(_touchPoint)); + ptr += sizeof(_touchPoint); + } else { + _touchPoint[0] = _touchPoint[1] = 0; + } // 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); } } diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h index 6d54b5cbcf..8e75ec8327 100644 --- a/interface/src/Transmitter.h +++ b/interface/src/Transmitter.h @@ -33,6 +33,8 @@ private: glm::vec3 _lastRotationRate; glm::vec3 _lastAcceleration; glm::vec3 _estimatedRotation; + uint16_t _touchPoint[2]; + char _touchState; #endif /* defined(__hifi__Transmitter__) */ }; From 0d231b55505b8128be92f2de950db2c0f42306fa Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 21 May 2013 23:24:58 -0700 Subject: [PATCH 3/5] Added lift to transmitter with finger, for true god-like flight! --- interface/src/Avatar.cpp | 10 ++++++++++ interface/src/Transmitter.h | 2 ++ 2 files changed, 12 insertions(+) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 7728b0bdfe..d4c16a3bb3 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -282,6 +282,8 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { const float TRANSMITTER_LATERAL_FORCE_SCALE = 25.f; 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(); } @@ -291,6 +293,14 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) { _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime; } + if (transmitter->getTouchState() == 'D') { + _thrust += THRUST_MAG * + (float)(transmitter->getTouchPoint()[1] - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF * + TRANSMITTER_LIFT_SCALE * + deltaTime * + _orientation.getUp(); + } + } } diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h index 8e75ec8327..8a8b19cb6a 100644 --- a/interface/src/Transmitter.h +++ b/interface/src/Transmitter.h @@ -26,6 +26,8 @@ public: const glm::vec3 getLastRotationRate() const { return _lastRotationRate; }; const glm::vec3 getLastAcceleration() const { return _lastRotationRate; }; const glm::vec3 getEstimatedRotation() const { return _estimatedRotation; }; + const uint16_t* getTouchPoint() const { return _touchPoint; }; + const char getTouchState() const { return _touchState; }; void processIncomingData(unsigned char* packetData, int numBytes); private: From fb553fce80c37ef044ffed3a82cd3ecdc762bc3a Mon Sep 17 00:00:00 2001 From: Geenz Date: Wed, 22 May 2013 10:53:35 -0400 Subject: [PATCH 4/5] Refactor startup timing a little by getting the current time the moment the application enters its main function, then passing that to Application's constructor. Also fix the titlebar bug by setting the title bar at the end of initializeGL() instead of at the end of the first frame (which would actually further the inaccuracy of the startup time). --- interface/src/Application.cpp | 23 +++++++------- interface/src/Application.h | 2 +- interface/src/main.cpp | 59 ++++++++++++++++++----------------- 3 files changed, 43 insertions(+), 41 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 8160d1c8db..e6119a71e7 100644 --- a/interface/src/Application.cpp +++ b/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) { diff --git a/interface/src/Application.h b/interface/src/Application.h index 8c9818eebe..e420426c63 100644 --- a/interface/src/Application.h +++ b/interface/src/Application.h @@ -46,7 +46,7 @@ class Application : public QApplication { public: static Application* getInstance() { return static_cast(QCoreApplication::instance()); } - Application(int& argc, char** argv); + Application(int& argc, char** argv, timeval &startup_time); void initializeGL(); void paintGL(); diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 58bad2db2f..ea915658af 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1,28 +1,31 @@ -// -// Interface -// -// Allows you to connect to and see/hear the shared 3D space. -// Optionally uses serialUSB connection to get gyro data for head movement. -// Optionally gets UDP stream from transmitter to animate controller/hand. -// -// Usage: The interface client first attempts to contact a domain server to -// discover the appropriate audio, voxel, and avatar servers to contact. -// Right now, the default domain server is "highfidelity.below92.com" -// You can change the domain server to use your own by editing the -// DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp -// -// -// Welcome Aboard! -// - -#include "Application.h" -#include "Log.h" - -int main(int argc, const char * argv[]) { - - Application app(argc, const_cast(argv)); - printLog( "Created QT Application.\n" ); - int exitCode = app.exec(); - printLog("Normal exit.\n"); - return exitCode; -} +// +// Interface +// +// Allows you to connect to and see/hear the shared 3D space. +// Optionally uses serialUSB connection to get gyro data for head movement. +// Optionally gets UDP stream from transmitter to animate controller/hand. +// +// Usage: The interface client first attempts to contact a domain server to +// discover the appropriate audio, voxel, and avatar servers to contact. +// Right now, the default domain server is "highfidelity.below92.com" +// You can change the domain server to use your own by editing the +// DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp +// +// +// Welcome Aboard! +// + +#include "Application.h" +#include "Log.h" + +int main(int argc, const char * argv[]) { + + timeval startup_time; + gettimeofday(&startup_time, NULL); + + Application app(argc, const_cast(argv), startup_time); + printLog( "Created QT Application.\n" ); + int exitCode = app.exec(); + printLog("Normal exit.\n"); + return exitCode; +} From 53dc81e309647785676ea9d1b9cb42a696afc9a9 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 22 May 2013 11:57:11 -0700 Subject: [PATCH 5/5] Fixes per code review. --- interface/src/Avatar.cpp | 4 ++-- interface/src/Transmitter.cpp | 33 ++++++++++++++++++--------------- interface/src/Transmitter.h | 11 +++++++---- 3 files changed, 27 insertions(+), 21 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index d4c16a3bb3..12d14aafdf 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -293,9 +293,9 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) { _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime; } - if (transmitter->getTouchState() == 'D') { + if (transmitter->getTouchState()->state == 'D') { _thrust += THRUST_MAG * - (float)(transmitter->getTouchPoint()[1] - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF * + (float)(transmitter->getTouchState()->y - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF * TRANSMITTER_LIFT_SCALE * deltaTime * _orientation.getUp(); diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp index 3a3a3f3ff8..5f6def92f7 100644 --- a/interface/src/Transmitter.cpp +++ b/interface/src/Transmitter.cpp @@ -31,21 +31,24 @@ void Transmitter::resetLevels() { } void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) { - if (numBytes >= 3 + sizeof(_lastRotationRate) + - sizeof(_lastAcceleration) + sizeof(_touchState)) { - unsigned char* ptr = &packetData[2]; - memcpy(&_lastRotationRate, ptr, sizeof(_lastRotationRate)); - ptr += sizeof(_lastRotationRate) + 1; - memcpy(&_lastAcceleration, ptr, sizeof(_lastAcceleration)); - ptr += sizeof(_lastAcceleration); - memcpy(&_touchState, ptr, sizeof(_touchState)); - ptr += sizeof(_touchState); - if (_touchState == 'D') { - memcpy(&_touchPoint, ptr, sizeof(_touchPoint)); - ptr += sizeof(_touchPoint); - } else { - _touchPoint[0] = _touchPoint[1] = 0; - } + 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; diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h index 8a8b19cb6a..326d0a1182 100644 --- a/interface/src/Transmitter.h +++ b/interface/src/Transmitter.h @@ -15,6 +15,11 @@ #include #include "world.h" +struct TouchState { + uint16_t x, y; + char state; +}; + class Transmitter { public: @@ -26,8 +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 uint16_t* getTouchPoint() const { return _touchPoint; }; - const char getTouchState() const { return _touchState; }; + const TouchState* getTouchState() const { return &_touchState; }; void processIncomingData(unsigned char* packetData, int numBytes); private: @@ -35,8 +39,7 @@ private: glm::vec3 _lastRotationRate; glm::vec3 _lastAcceleration; glm::vec3 _estimatedRotation; - uint16_t _touchPoint[2]; - char _touchState; + TouchState _touchState; #endif /* defined(__hifi__Transmitter__) */ };