diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index 78ea7f950c..d69a3fa35f 100644 --- a/interface/src/Head.cpp +++ b/interface/src/Head.cpp @@ -73,25 +73,43 @@ void Head::reset() void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int head_mirror, glm::vec3 * gravity) // Using serial data, update avatar/render position and angles { + + const float PITCH_ACCEL_COUPLING = 0.5; + const float ROLL_ACCEL_COUPLING = -1.0; float measured_pitch_rate = serialInterface->getRelativeValue(PITCH_RATE); float measured_yaw_rate = serialInterface->getRelativeValue(YAW_RATE); - float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X); - float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z); + float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X) - + ROLL_ACCEL_COUPLING*serialInterface->getRelativeValue(ROLL_RATE); + float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z) - + PITCH_ACCEL_COUPLING*serialInterface->getRelativeValue(PITCH_RATE); float measured_roll_rate = serialInterface->getRelativeValue(ROLL_RATE); + //std::cout << "Pitch Rate: " << serialInterface->getRelativeValue(PITCH_RATE) << + // " fwd_accel: " << serialInterface->getRelativeValue(ACCEL_Z) << "\n"; + //std::cout << "Roll Rate: " << serialInterface->getRelativeValue(ROLL_RATE) << + //" ACCEL_X: " << serialInterface->getRelativeValue(ACCEL_X) << "\n"; + //std::cout << "Pitch: " << Pitch << "\n"; + // Update avatar head position based on measured gyro rates - const float HEAD_ROTATION_SCALE = 0.80; - const float HEAD_ROLL_SCALE = 0.80; - const float HEAD_LEAN_SCALE = 0.03; - if (head_mirror) { - addYaw(-measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); + const float HEAD_ROTATION_SCALE = 0.70; + const float HEAD_ROLL_SCALE = 0.40; + const float HEAD_LEAN_SCALE = 0.01; + const float MAX_PITCH = 45; + const float MIN_PITCH = -45; + const float MAX_YAW = 85; + const float MIN_YAW = -85; + + if ((Pitch < MAX_PITCH) && (Pitch > MIN_PITCH)) addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - addRoll(-measured_roll_rate * HEAD_ROLL_SCALE * frametime); + addRoll(-measured_roll_rate * HEAD_ROLL_SCALE * frametime); + + if (head_mirror) { + if ((Yaw < MAX_YAW) && (Yaw > MIN_YAW)) + addYaw(-measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); addLean(-measured_lateral_accel * frametime * HEAD_LEAN_SCALE, -measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } else { - addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); - addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - addRoll(-measured_roll_rate * HEAD_ROLL_SCALE * frametime); + if ((Yaw < MAX_YAW) && (Yaw > MIN_YAW)) + addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); addLean(measured_lateral_accel * frametime * -HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } } @@ -116,9 +134,9 @@ void Head::simulate(float deltaTime) if (!noise) { // Decay back toward center - Pitch *= (1.f - DECAY*deltaTime); - Yaw *= (1.f - DECAY*deltaTime); - Roll *= (1.f - DECAY*deltaTime); + Pitch *= (1.f - DECAY*2*deltaTime); + Yaw *= (1.f - DECAY*2*deltaTime); + Roll *= (1.f - DECAY*2*deltaTime); } else { // Move toward new target