diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 9ca648a44a..12d14aafdf 100644 --- a/interface/src/Avatar.cpp +++ b/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(); + } + } } diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index f4aa5c9785..67355bda50 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.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 diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp index 917c98847f..5f6def92f7 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; @@ -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); } } diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h index 6d54b5cbcf..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,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__) */ };