From 6ff4b88ea4da0da9b501a9b364f9856cdfc01d84 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 21 May 2013 15:46:11 -0700 Subject: [PATCH 1/4] 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/4] 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/4] 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 53dc81e309647785676ea9d1b9cb42a696afc9a9 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 22 May 2013 11:57:11 -0700 Subject: [PATCH 4/4] 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__) */ };