diff --git a/examples/c/test_opengl.cpp b/examples/c/test_opengl.cpp index 119da526..ef879239 100644 --- a/examples/c/test_opengl.cpp +++ b/examples/c/test_opengl.cpp @@ -90,7 +90,7 @@ Tracker::Tracker() m_fusion(psmove_fusion_new(m_tracker, 1., 1000.)) { psmove_tracker_set_mirror(m_tracker, PSMove_True); - psmove_tracker_set_exposure(m_tracker, Exposure_HIGH); + psmove_tracker_set_exposure(m_tracker, Exposure_LOW); m_moves = (PSMove**)calloc(m_count, sizeof(PSMove*)); m_items = (int*)calloc(m_count, sizeof(int)); diff --git a/external/MadgwickAHRS/MadgwickAHRS.c b/external/MadgwickAHRS/MadgwickAHRS.c deleted file mode 100644 index 6bfdf071..00000000 --- a/external/MadgwickAHRS/MadgwickAHRS.c +++ /dev/null @@ -1,249 +0,0 @@ -//===================================================================================================== -// MadgwickAHRS.c -//===================================================================================================== -// -// Implementation of Madgwick's IMU and AHRS algorithms. -// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised -// 02/08/2012 Thomas Perl Modifications for PS Move API integration -// 30/05/2013 Jan Ciger Use normal invSqrt function for 64-bit compat -// -//===================================================================================================== - -//--------------------------------------------------------------------------------------------------- -// Header files - -#include "MadgwickAHRS.h" -#include - -//--------------------------------------------------------------------------------------------------- -// Definitions - -#define betaDef 0.5f // 2 * proportional gain - -//--------------------------------------------------------------------------------------------------- -// Variable definitions - -volatile float beta = betaDef; // 2 * proportional gain (Kp) - -//--------------------------------------------------------------------------------------------------- -// Function declarations - -float invSqrt(float x); - -//==================================================================================================== -// Functions - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update - -void MadgwickAHRSupdate(float *quaternion, float sampleFreq, - float ax, float ay, float az, - float gx, float gy, float gz, - float mx, float my, float mz) -{ - float q0 = quaternion[0], - q1 = quaternion[1], - q2 = quaternion[2], - q3 = quaternion[3]; - - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - MadgwickAHRSupdateIMU(quaternion, sampleFreq, ax, ay, az, gx, gy, gz); - return; - } - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0mx = 2.0f * q0 * mx; - _2q0my = 2.0f * q0 * my; - _2q0mz = 2.0f * q0 * mz; - _2q1mx = 2.0f * q1 * mx; - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _2q0q2 = 2.0f * q0 * q2; - _2q2q3 = 2.0f * q2 * q3; - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; - - // Reference direction of Earth's magnetic field - hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; - hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; - _2bx = (float)sqrt(hx * hx + hy * hy); - _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; - _4bx = 2.0f * _2bx; - _4bz = 2.0f * _2bz; - - // Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * (1.0f / sampleFreq); - q1 += qDot2 * (1.0f / sampleFreq); - q2 += qDot3 * (1.0f / sampleFreq); - q3 += qDot4 * (1.0f / sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - quaternion[0] = q0; - quaternion[1] = q1; - quaternion[2] = q2; - quaternion[3] = q3; -} - -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void MadgwickAHRSupdateIMU(float *quaternion, float sampleFreq, - float ax, float ay, float az, - float gx, float gy, float gz) -{ - float q0 = quaternion[0], - q1 = quaternion[1], - q2 = quaternion[2], - q3 = quaternion[3]; - - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - // Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * (1.0f / sampleFreq); - q1 += qDot2 * (1.0f / sampleFreq); - q2 += qDot3 * (1.0f / sampleFreq); - q3 += qDot4 * (1.0f / sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - quaternion[0] = q0; - quaternion[1] = q1; - quaternion[2] = q2; - quaternion[3] = q3; -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -// 2013-05-30: Replaced with normal inverse square root for 64-bit compat - -float invSqrt(float x) { - return 1.0f / (float)sqrt(x); -} - -//==================================================================================================== -// END OF CODE -//==================================================================================================== diff --git a/external/MadgwickAHRS/MadgwickAHRS.h b/external/MadgwickAHRS/MadgwickAHRS.h deleted file mode 100644 index 775bc5b1..00000000 --- a/external/MadgwickAHRS/MadgwickAHRS.h +++ /dev/null @@ -1,49 +0,0 @@ -//===================================================================================================== -// MadgwickAHRS.h -//===================================================================================================== -// -// Implementation of Madgwick's IMU and AHRS algorithms. -// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// 02/08/2012 Thomas Perl Modifications for PS Move API integration -// -//===================================================================================================== -#ifndef MadgwickAHRS_h -#define MadgwickAHRS_h - -#ifdef __cplusplus -extern "C" { -#endif - - -//---------------------------------------------------------------------------------------------------- -// Variable declaration - -extern volatile float beta; // algorithm gain - -//--------------------------------------------------------------------------------------------------- -// Function declarations - -// quaternion -> pointer to a float[4] -// sampleFreq -> sample frequency in Hz - -void MadgwickAHRSupdate(float *quaternion, float sampleFreq, - float ax, float ay, float az, - float gx, float gy, float gz, - float mx, float my, float mz); - -void MadgwickAHRSupdateIMU(float *quaternion, float sampleFreq, - float ax, float ay, float az, - float gx, float gy, float gz); - -#ifdef __cplusplus -} -#endif - -#endif -//===================================================================================================== -// End of file -//===================================================================================================== diff --git a/external/MahonyAHRS/MahonyAHRS.c b/external/MahonyAHRS/MahonyAHRS.c deleted file mode 100644 index 2ec12f06..00000000 --- a/external/MahonyAHRS/MahonyAHRS.c +++ /dev/null @@ -1,232 +0,0 @@ -//===================================================================================================== -// MahonyAHRS.c -//===================================================================================================== -// -// Madgwick's implementation of Mayhony's AHRS algorithm. -// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// -//===================================================================================================== - -//--------------------------------------------------------------------------------------------------- -// Header files - -#include "MahonyAHRS.h" -#include - -//--------------------------------------------------------------------------------------------------- -// Definitions - -#define sampleFreq 512.0f // sample frequency in Hz -#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain -#define twoKiDef (2.0f * 0.0f) // 2 * integral gain - -//--------------------------------------------------------------------------------------------------- -// Variable definitions - -volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp) -volatile float twoKi = twoKiDef; // 2 * integral gain (Ki) -volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame -volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki - -//--------------------------------------------------------------------------------------------------- -// Function declarations - -float invSqrt(float x); - -//==================================================================================================== -// Functions - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update - -void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - float recipNorm; - float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - float hx, hy, bx, bz; - float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; - float halfex, halfey, halfez; - float qa, qb, qc; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az); - return; - } - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; - - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - bx = sqrt(hx * hx + hy * hy); - bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); - - // Estimated direction of gravity and magnetic field - halfvx = q1q3 - q0q2; - halfvy = q0q1 + q2q3; - halfvz = q0q0 - 0.5f + q3q3; - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); - halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); - halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); - - // Compute and apply integral feedback if enabled - if(twoKi > 0.0f) { - integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki - integralFBy += twoKi * halfey * (1.0f / sampleFreq); - integralFBz += twoKi * halfez * (1.0f / sampleFreq); - gx += integralFBx; // apply integral feedback - gy += integralFBy; - gz += integralFBz; - } - else { - integralFBx = 0.0f; // prevent integral windup - integralFBy = 0.0f; - integralFBz = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - // Integrate rate of change of quaternion - gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors - gy *= (0.5f * (1.0f / sampleFreq)); - gz *= (0.5f * (1.0f / sampleFreq)); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { - float recipNorm; - float halfvx, halfvy, halfvz; - float halfex, halfey, halfez; - float qa, qb, qc; - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and vector perpendicular to magnetic flux - halfvx = q1 * q3 - q0 * q2; - halfvy = q0 * q1 + q2 * q3; - halfvz = q0 * q0 - 0.5f + q3 * q3; - - // Error is sum of cross product between estimated and measured direction of gravity - halfex = (ay * halfvz - az * halfvy); - halfey = (az * halfvx - ax * halfvz); - halfez = (ax * halfvy - ay * halfvx); - - // Compute and apply integral feedback if enabled - if(twoKi > 0.0f) { - integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki - integralFBy += twoKi * halfey * (1.0f / sampleFreq); - integralFBz += twoKi * halfez * (1.0f / sampleFreq); - gx += integralFBx; // apply integral feedback - gy += integralFBy; - gz += integralFBz; - } - else { - integralFBx = 0.0f; // prevent integral windup - integralFBy = 0.0f; - integralFBz = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - // Integrate rate of change of quaternion - gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors - gy *= (0.5f * (1.0f / sampleFreq)); - gz *= (0.5f * (1.0f / sampleFreq)); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root - -float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} - -//==================================================================================================== -// END OF CODE -//==================================================================================================== diff --git a/external/MahonyAHRS/MahonyAHRS.h b/external/MahonyAHRS/MahonyAHRS.h deleted file mode 100644 index 9000997d..00000000 --- a/external/MahonyAHRS/MahonyAHRS.h +++ /dev/null @@ -1,32 +0,0 @@ -//===================================================================================================== -// MahonyAHRS.h -//===================================================================================================== -// -// Madgwick's implementation of Mayhony's AHRS algorithm. -// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// -//===================================================================================================== -#ifndef MahonyAHRS_h -#define MahonyAHRS_h - -//---------------------------------------------------------------------------------------------------- -// Variable declaration - -extern volatile float twoKp; // 2 * proportional gain (Kp) -extern volatile float twoKi; // 2 * integral gain (Ki) -extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame - -//--------------------------------------------------------------------------------------------------- -// Function declarations - -void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); -void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); - -#endif -//===================================================================================================== -// End of file -//===================================================================================================== diff --git a/include/psmove.h b/include/psmove.h index c38d3b96..b8ca4770 100644 --- a/include/psmove.h +++ b/include/psmove.h @@ -165,6 +165,25 @@ enum PSMove_RemoteConfig { PSMove_OnlyRemote = 2, /*!< Use only remote (moved) devices, ignore local devices */ }; +enum PSMoveOrientation_Fusion_Type { + OrientationFusion_None, + OrientationFusion_MadgwickIMU, + OrientationFusion_MadgwickMARG, + OrientationFusion_ComplementaryMARG, +}; + +/*! Common Calibration Poses */ +enum PSMove_CalibrationPose_Type { + CalibrationPose_Upright, + CalibrationPose_LyingFlat +}; + +/*! Coordinate systems to use for the sensor data */ +enum PSMove_SensorDataBasis_Type { + SensorDataBasis_Native, + SensorDataBasis_OpenGL, +}; + #ifndef SWIG struct _PSMove; typedef struct _PSMove PSMove; /*!< Handle to a PS Move Controller. @@ -183,6 +202,36 @@ typedef struct { unsigned char dev_info[38]; } PSMove_Ext_Device_Info; +/* A 3d vector - methods in math/psmove_vector.h */ +struct _PSMove_3AxisVector +{ + union { + struct + { + float x; + float y; + float z; + }; + float v[3]; + }; +}; +typedef struct _PSMove_3AxisVector PSMove_3AxisVector; + +/* A 3x3 transform matrix - methods in math/psmove_vector.h */ +struct _PSMove_3AxisTransform +{ + union { + struct + { + float row0[3]; + float row1[3]; + float row2[3]; + }; + float m[9]; + }; +}; +typedef struct _PSMove_3AxisTransform PSMove_3AxisTransform; + /*! Library version number */ enum PSMove_Version { /** @@ -194,7 +243,7 @@ enum PSMove_Version { **/ PSMOVE_CURRENT_VERSION = 0x030001, /*!< Current version, see psmove_init() */ }; - + /** * \brief Initialize the library and check for the right version * @@ -890,6 +939,29 @@ ADDAPI void ADDCALL psmove_get_gyroscope_frame(PSMove *move, enum PSMove_Frame frame, float *gx, float *gy, float *gz); +/** + * \brief Get the raw magnetometer reading from the PS Move. + * + * This function reads the raw sensor values from the controller, + * pointing to magnetic north. + * + * The result value range is -2048..+2047. The magnetometer is located + * roughly below the glowing orb - you can glitch the values with a + * strong kitchen magnet by moving it around the bottom ring of the orb. + * You can detect if a magnet is nearby by checking if any two values + * stay at zero for several frames. + * + * You need to call psmove_poll() first to read new data from the + * controller. + * + * \param move A valid \ref PSMove handle + * \param mx Pointer to store the raw X axis reading, or \c NULL + * \param my Pointer to store the raw Y axis reading, or \c NULL + * \param mz Pointer to store the raw Z axis reading, or \c NULL + **/ +ADDAPI void +ADDCALL psmove_get_magnetometer(PSMove *move, int *mx, int *my, int *mz); + /** * \brief Get the normalized magnetometer vector from the controller. * @@ -908,8 +980,62 @@ ADDCALL psmove_get_gyroscope_frame(PSMove *move, enum PSMove_Frame frame, * \param mz Pointer to store the Z axis reading, or \c NULL **/ ADDAPI void -ADDCALL psmove_get_magnetometer_vector(PSMove *move, - float *mx, float *my, float *mz); +ADDCALL psmove_get_magnetometer_vector(PSMove *move, float *mx, float *my, float *mz); + +/** + * \brief Get the normalized magnetometer vector from the controller. + * + * The normalized magnetometer vector is a three-axis vector where each + * component is in the range [-1,+1], including both endpoints. The range + * will be dynamically determined based on the highest (and lowest) value + * observed during runtime. To get the raw magnetometer readings, use + * psmove_get_magnetometer(). + * + * You need to call psmove_poll() first to read new data from the + * controller. + * + * \param move A valid \ref PSMove handle + * \param out_m The output \ref PSMove_3AxisVector + **/ +ADDAPI void +ADDCALL psmove_get_magnetometer_3axisvector(PSMove *move, PSMove_3AxisVector *out_m); + +/** + * \brief Reset the magnetometer calibration state. + * + * This will reset the magnetometer calibration data, so they can be + * re-adjusted dynamically. Used by the calibration utility. + * + * \ref move A valid \ref PSMove handle + **/ +ADDAPI void +ADDCALL psmove_reset_magnetometer_calibration(PSMove *move); + +/** + * \brief Save the magnetometer calibration values. + * + * This will save the magnetometer calibration data to persistent storage. + * If a calibration already exists, this will overwrite the old values. + * + * \param move A valid \ref PSMove handle + **/ +ADDAPI void +ADDCALL psmove_save_magnetometer_calibration(PSMove *move); + +/** + * \brief Return the raw magnetometer calibration range. + * + * The magnetometer calibration is dynamic at runtime - this function returns + * the raw range of magnetometer calibration. The user should rotate the + * controller in all directions to find the response range of the controller + * (this will be dynamically adjusted). + * + * \param move A valid \ref PSMove handle + * + * \return The smallest raw sensor range difference of all three axes + **/ +ADDAPI float +ADDCALL psmove_get_magnetometer_calibration_range(PSMove *move); /** * \brief Check if calibration is available on this controller. @@ -1027,43 +1153,227 @@ ADDCALL psmove_get_orientation(PSMove *move, ADDAPI void ADDCALL psmove_reset_orientation(PSMove *move); +/** + * \brief Set the orientation fusion algorithm to use. + * + * There are currently the following orientation filter algorithms available: + * OrientationFusion_MadgwickIMU - Classic MadgwickIMU: Gyro integration + Gravity correction + * - 1st Least expensive + * - Con: Suffers from pretty bad drift + * OrientationFusion_MadgwickMARG - Classic MadgwickMARG: Gyro integration + Gravity/Magnetometer correction + * - 2nd least expensive + * - Con: Suffers from slow drift about the yaw + * OrientationFusion_ComplementaryMARG - Gyro integration blended with optimized Gravity/Magnetometer alignment + * - Suffers no drift + * - Con: Most expensive algorithms of the three (but not horrendously so) + + * \param move A valid \ref PSMove handle + * \param fusion_type The orientation fusion algorithm denoted by the \ref PSMoveOrientation_Fusion_Type enum + **/ +ADDAPI void +ADDCALL psmove_set_orientation_fusion_type(PSMove *move, enum PSMoveOrientation_Fusion_Type fusion_type); /** - * \brief Reset the magnetometer calibration state. + * \brief Set a common transform used on the calibration data in the psmove_get_transform__... methods * - * This will reset the magnetometer calibration data, so they can be - * re-adjusted dynamically. Used by the calibration utility. + * This method sets the transform used to modify the calibration vectors returned by: + * - psmove_orientation_get_magnetometer_calibration_direction() + * - psmove_orientation_get_gravity_calibration_direction() * - * \ref move A valid \ref PSMove handle + * The transformed calibration data is used by the orientation filter to compute + * a quaternion (see \ref psmove_orientation_get_quaternion) that represents + * the controllers current rotation from the "identity pose". + * + * Historically, the "identity pose" bas been with the controller laying flat + * with the controller pointing at the screen. However, now that we have a + * calibration step that record the magnetic field direction relative to + * gravity it makes more sense to make the identity pose with the controller + * sitting vertically since it's more stable to record that way. + * + * In order to maintain reverse compatibility, this transform defaults to rotating + * the vertically recorded calibration vectors 90 degrees about X axis as if the + * controller was laying flat during calibration. + * + * Therefore, if you want a different "identity pose" then the default, + * use this method to set a custom transform. + * + * There are the following transforms available: + * - CalibrationPose_Upright - "identity pose" is the controller standing upright + * - CalibrationPose_LyingFlat - "identity pose" is the controller laying down pointed at the screen + * + * \param orientation_state A valid \ref PSMoveOrientation handle + * \param transform A \ref PSMove_CalibrationPose_Type common transform to apply to the calibration data **/ ADDAPI void -ADDCALL psmove_reset_magnetometer_calibration(PSMove *move); +ADDCALL psmove_set_calibration_pose(PSMove *move, enum PSMove_CalibrationPose_Type calibration_pose); /** - * \brief Save the magnetometer calibration values. +* \brief Set the custom transform used on the calibration data in the psmove_get_transform__... methods +* +* \param orientation_state A valid \ref PSMoveOrientation handle +* \param transform A \ref PSMove_3AxisTransform transform to apply to the calibration data +**/ +ADDAPI void +ADDCALL psmove_set_calibration_transform(PSMove *move, const PSMove_3AxisTransform *transform); + +/** +* \brief Get the native earth gravity direction. +* +* This returns the native direction of the gravitational field in the identity pose during calibration. +* +* \param move A valid \ref PSMove handle +* +* \return The expected direction of gravity +**/ +ADDAPI void +ADDCALL psmove_get_identity_gravity_calibration_direction(PSMove *move, PSMove_3AxisVector *out_a); + +/** +* \brief Get the transformed earth gravity direction. +* +* This returns the direction of the gravitational field in the transformed identity pose. +* +* \param move A valid \ref PSMove handle +* +* \return The transformed expected direction of gravity +**/ +ADDAPI void +ADDCALL psmove_get_transformed_gravity_calibration_direction(PSMove *move, PSMove_3AxisVector *out_a); + +/** +* \brief Get the calibration magnetometer direction. +* +* This returns the direction of the magnetic field in the un-transformed identity pose. +* +* \param move A valid \ref PSMove handle +* +* \return The direction of the magnetic field +**/ +ADDAPI void +ADDCALL psmove_get_identity_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *out_m); + +/** +* \brief Get the transformed calibration magnetometer direction. +* +* This returns the direction of the magnetic field in the transformed identity pose. +* +* \param move A valid \ref PSMove handle +* \param out_m The output \ref PSMove_3AxisVector +**/ +ADDAPI void +ADDCALL psmove_get_transformed_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *out_m); + +/** +* \brief Set the calibration magnetometer direction. +* +* This sets the direction of the magnetic field in the identity pose. +* This is typically only set during calibration. +* +* \param move A valid \ref PSMove handle +* \param m A valid \ref PSMoveVector +**/ +ADDAPI void +ADDCALL psmove_set_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *m); + +/** + * \brief Set the transform used on the sensor data in the psmove_get_transform__... methods * - * This will save the magnetometer calibration data to persistent storage. - * If a calibration already exists, this will overwrite the old values. + * This method sets the transform used to modify the sensor vectors returned by: + * - psmove_get_transformed_magnetometer_3axisvector() + * - psmove_get_transformed_accelerometer_frame_3axisvector() + * - psmove_get_transformed_accelerometer_frame_direction() + * - psmove_get_transformed_gyroscope_frame_3axisvector() + * + * The transformed sensor data is used by the orientation filter to compute + * a quaternion (see \ref psmove_orientation_get_quaternion) that represents + * the controllers current rotation from the "identity pose". + * + * Historically, the sensor data in the orientation code has been rotated 90 degrees + * clockwise about the x-axis. The original Madgwick orientation filter was coded to assume + * an OpenGL style coordinate system (+x=right, +y=up, +z=out of screen), rather than + * than PSMoves coordinate system where: + * + * +x = From Select to Start button + * +y = From Trigger to Move button + * +z = From glowing orb to USB connector + * + * The current default sets the sensor transform to assume an OpenGL style coordinate system + * in order to maintain reverse compatibility + * + * There are the following transforms available: + * - SensorDataBasis_Native - Keep the sensor data as it was + * - SensorDataBasis_OpenGL - Rotate 90 degrees about the x-axis (historical default) + * + * \param orientation_state A valid \ref PSMoveOrientation handle + * \param transform A \ref PSMove_SensorDataBasis_Type transform to apply to the sensor data + **/ +ADDAPI void +ADDCALL psmove_set_sensor_data_basis(PSMove *move, enum PSMove_SensorDataBasis_Type basis_type); + +/** +* \brief Set the transform used on the sensor data in the psmove_get_transform__... methods +* +* \param orientation_state A valid \ref PSMoveOrientation handle +* \param transform A \ref PSMove_3AxisTransform transform to apply to the sensor data +**/ +ADDAPI void +ADDCALL psmove_set_sensor_data_transform(PSMove *move, const PSMove_3AxisTransform *transform); + +/** + * \brief Get the transformed current magnetometer direction. + * + * This returns the current normalized direction of the magnetic field with the sensor transform applied. + * + * You need to call psmove_poll() first to read new data from the + * controller. * * \param move A valid \ref PSMove handle + * \param out_m The output \ref PSMove_3AxisVector **/ ADDAPI void -ADDCALL psmove_save_magnetometer_calibration(PSMove *move); +ADDCALL psmove_get_transformed_magnetometer_direction(PSMove *move, PSMove_3AxisVector *out_m); /** - * \brief Return the raw magnetometer calibration range. + * \brief Get the transformed current accelerometer vector. * - * The magnetometer calibration is dynamic at runtime - this function returns - * the raw range of magnetometer calibration. The user should rotate the - * controller in all directions to find the response range of the controller - * (this will be dynamically adjusted). + * This returns the current non-normalized vector of the accelerometer with the sensor transform applied. + * + * You need to call psmove_poll() first to read new data from the + * controller. * * \param move A valid \ref PSMove handle + * \param out_a The output \ref PSMove_3AxisVector + **/ +ADDAPI void +ADDCALL psmove_get_transformed_accelerometer_frame_3axisvector(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_a); + +/** + * \brief Get the transformed normalized current accelerometer direction. * - * \return The smallest raw sensor range difference of all three axes + * This returns the current normalized direction of the accelerometer with the sensor transform applied. + * + * You need to call psmove_poll() first to read new data from the + * controller. + * + * \param move A valid \ref PSMove handle + * \param out_a The output \ref PSMove_3AxisVector **/ -ADDAPI int -ADDCALL psmove_get_magnetometer_calibration_range(PSMove *move); +ADDAPI void +ADDCALL psmove_get_transformed_accelerometer_frame_direction(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_a); + +/** + * \brief Get the transformed current gyroscope vector. + * + * This returns the current gyroscope vector (omega) with the sensor transform applied. + * + * You need to call psmove_poll() first to read new data from the + * controller. + * + * \param move A valid \ref PSMove handle + * \param out_w The output \ref PSMove_3AxisVector + **/ +ADDAPI void +ADDCALL psmove_get_transformed_gyroscope_frame_3axisvector(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_w); /** * \brief Disconnect from the PS Move and release resources. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 61fbe91b..23029617 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -15,7 +15,7 @@ set(PSMOVEAPI_VERSION_SONAME "${PSMOVEAPI_VERSION_MAJOR}") set(PSMOVEAPI_REQUIRED_LIBS) # Container for library dependencies set(PSMOVEAPI_PLATFORM_SRC) # Container for source files set(PSMOVEAPI_INSTALL_TARGETS) # Container for build targets to be installed - +set(PSMOVEAPI_MATH_SRC) # Container for math related source files include_directories(${ROOT_DIR}/external/hidapi/hidapi) # Put any other libraries that you might need in here @@ -23,22 +23,6 @@ link_directories(${ROOT_DIR}/external/libs) set(INFO_LICENSE "BSD") -# Madgwick's orientation algorithm (GPL-licensed) -option(PSMOVE_USE_MADGWICK_AHRS "Use AHRS algorithm (GPL license)" OFF) - -# AHRS algorithm -IF(PSMOVE_USE_MADGWICK_AHRS) - add_definitions(-DPSMOVE_WITH_MADGWICK_AHRS) - set(INFO_LICENSE "GPL") - set(INFO_AHRS_ALGORITHM "Madgwick AHRS") - set(PSMOVEAPI_ALGORITHM_SRC - ${ROOT_DIR}/external/MadgwickAHRS/MadgwickAHRS.c - ) -ELSE() - set(INFO_AHRS_ALGORITHM "None") - set(PSMOVEAPI_ALGORITHM_SRC) -ENDIF() - # Make a debug build with helpful output for debugging / maintenance option(PSMOVE_USE_DEBUG "Build for debugging" OFF) @@ -154,7 +138,8 @@ source_group("Source Files\\daemon" FILES ${PSMOVEAPI_MOVED_SRC}) file(GLOB PSMOVEAPI_SRC "${CMAKE_CURRENT_LIST_DIR}/*.c" - "${CMAKE_CURRENT_LIST_DIR}/*.h" + "${CMAKE_CURRENT_LIST_DIR}/*.cpp" + "${CMAKE_CURRENT_LIST_DIR}/*.h" ) list(APPEND PSMOVEAPI_SRC @@ -165,6 +150,19 @@ file(GLOB PSMOVEAPI_HEADERS "${ROOT_DIR}/include/*.h" ) +file (GLOB PSMOVEAPI_MATH_HEADERS + "${CMAKE_CURRENT_LIST_DIR}/math/*.h" + "${CMAKE_CURRENT_LIST_DIR}/math/*.hpp" +) + +file (GLOB PSMOVEAPI_MATH_SRC + "${CMAKE_CURRENT_LIST_DIR}/math/*.c" + "${CMAKE_CURRENT_LIST_DIR}/math/*.cpp" +) + +source_group("Header Files\\math" FILES ${PSMOVEAPI_MATH_HEADERS}) +source_group("Source Files\\math" FILES ${PSMOVEAPI_MATH_SRC}) + # Source files that are needed for both the shared and static library set(PSMOVEAPI_LIBRARY_SRC) @@ -174,6 +172,8 @@ list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_MOVED_SRC}) list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_MOVED_HEADER}) list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_PLATFORM_SRC}) list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_ALGORITHM_SRC}) +list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_MATH_HEADERS}) +list(APPEND PSMOVEAPI_LIBRARY_SRC ${PSMOVEAPI_MATH_SRC}) # Shared library diff --git a/src/math/psmove_alignment.cpp b/src/math/psmove_alignment.cpp new file mode 100644 index 00000000..d098dd45 --- /dev/null +++ b/src/math/psmove_alignment.cpp @@ -0,0 +1,185 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +//-- includes ---- +#include + +#include "psmove_alignment.hpp" +#include "psmove_quaternion.hpp" +#include "psmove_math.h" + +//-- public methods ---- +// This formula comes from Sebastian O.H. Madgwick's 2010 paper: +// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" +// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf +void +psmove_alignment_compute_objective_vector( + const glm::quat &q, const glm::vec3 &d, const glm::vec3 &s, + glm::vec3 &out_f, float *out_squared_error) +{ + // Computing f(q;d, s) = (q^-1 * d * q) - s + glm::vec3 d_rotated = psmove_vector3f_clockwise_rotate(q, d); + + out_f = d_rotated - s; + + if (out_squared_error) + { + *out_squared_error = glm::length(out_f); + } +} + +// This formula comes from Sebastian O.H. Madgwick's 2010 paper: +// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" +// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf +void +psmove_alignment_compute_objective_jacobian( + const glm::quat &q, const glm::vec3 &d, glm::mat3x4 &J) +{ + /* + * The Jacobian of a function is a matrix of partial derivatives that relates rates of changes in inputs to outputs + * + * In this case the inputs are the components of the quaternion q (qw, qx, qy, and qz) + * and the outputs are the components of the error vector f(fx, fy, fz) + * Where f= q^-1*[0 dx dy dz]*q - s, d= initial field vector, s= target field vector + * + * Since there are 4 input vars (q1,q2,q3,q4) and 3 output vars (fx, fy, fz) + * The Jacobian is a 3x4 matrix that looks like this: + * + * | df_x/dq_1 df_x/dq_2 df_x/dq_3 df_x/dq_4 | + * | df_y/dq_1 df_y/dq_2 df_y/dq_3 df_y/dq_4 | + * | df_z/dq_1 df_z/dq_2 df_z/dq_3 df_z/dq_4 | + */ + + const float two_dxq1 = 2.f*d.x*q.w; + const float two_dxq2 = 2.f*d.x*q.x; + const float two_dxq3 = 2.f*d.x*q.y; + const float two_dxq4 = 2.f*d.x*q.z; + + const float two_dyq1 = 2.f*d.y*q.w; + const float two_dyq2 = 2.f*d.y*q.x; + const float two_dyq3 = 2.f*d.y*q.y; + const float two_dyq4 = 2.f*d.y*q.z; + + const float two_dzq1 = 2.f*d.z*q.w; + const float two_dzq2 = 2.f*d.z*q.x; + const float two_dzq3 = 2.f*d.z*q.y; + const float two_dzq4 = 2.f*d.z*q.z; + + // From Eqn 22 on page 8 + // This saved out transposed (4 rows x 3 columns) + J[0][0] = two_dyq4 - two_dzq3; J[1][0] = -two_dxq4 + two_dzq2; J[2][0] = two_dxq3 - two_dyq2; + J[0][1] = two_dyq3 + two_dzq4; J[1][1] = two_dxq3 - 2.f*two_dyq2 + two_dzq1; J[2][1] = two_dxq4 - two_dyq1 - 2.f*two_dzq2; + J[0][2] = -2.f*two_dxq3 + two_dyq2 - two_dzq1; J[1][2] = two_dxq2 + two_dzq4; J[2][2] = two_dxq1 + two_dyq4 - 2.f*two_dzq3; + J[0][3] = -2.f*two_dxq4 + two_dyq1 + two_dzq2; J[1][3] = -two_dxq1 - 2.f*two_dyq4 + two_dzq3; J[2][3] = two_dxq2 + two_dyq3; +} + +bool +psmove_alignment_quaternion_between_vector_frames( + const glm::vec3* from[2], const glm::vec3* to[2], const float tolerance, const glm::quat &initial_q, + glm::quat &out_q) +{ + bool success = true; + + glm::quat previous_q = initial_q; + glm::quat q = initial_q; + + //const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared + const int k_max_iterations = 32; + float previous_error_squared = k_real_max; + float error_squared = k_real_max; + float squared_error_delta = k_real_max; + float gamma = 0.5f; + bool backtracked = false; + + for (int iteration = 0; + iteration < k_max_iterations && // Haven't exceeded iteration limit + error_squared > tolerance && // Aren't within tolerance of the destination + squared_error_delta > k_normal_epsilon && // Haven't reached a minima + gamma > k_normal_epsilon; // Haven't reduced our step size to zero + iteration++) + { + // Compute the two 3x1 halves of the 6x1 objective function matrix |f_0| + // |f_1| + float error_squared0, error_squared1; + + glm::vec3 f_0; + psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0); + + glm::vec3 f_1; + psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1); + + error_squared = error_squared0 + error_squared1; + + // Make sure this new step hasn't made the error worse + if (error_squared <= previous_error_squared) + { + // We won't have a valid error derivative if we had to back track + squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta; + backtracked = false; + + // This is a good step. + // Remember it in case the next one makes things worse + previous_error_squared = error_squared; + previous_q = q; + + // Compute the two 4x3 halves of the 4x6 objective function Jacobian matrix: [J_0|J_1] + glm::mat3x4 J_0; + psmove_alignment_compute_objective_jacobian(q, *from[0], J_0); + + glm::mat3x4 J_1; + psmove_alignment_compute_objective_jacobian(q, *from[1], J_1); + + // Compute the gradient of the objective function + glm::vec4 gradient_f = J_0*f_0 + J_1*f_1; + glm::quat gradient_q(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); + + // The gradient points toward the maximum, so we subtract it off to head toward the minimum. + // The step scale 'gamma' is just a guess. + q = q + gradient_q*(-gamma); //q-= gradient_q*gamma; + q = glm::normalize(q); + } + else + { + // The step made the error worse. + // Return to the previous orientation and half our step size. + q = previous_q; + gamma /= 2.f; + backtracked = true; + } + } + + if (error_squared > tolerance) + { + // Make sure we didn't fail to converge on the goal + success = false; + } + + out_q= q; + + return success; +} \ No newline at end of file diff --git a/src/math/psmove_alignment.hpp b/src/math/psmove_alignment.hpp new file mode 100644 index 00000000..fd9c065f --- /dev/null +++ b/src/math/psmove_alignment.hpp @@ -0,0 +1,59 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +/* General math utility methods for aligning frames of reference */ +#ifndef __PSMOVE_ALIGNMENT_H +#define __PSMOVE_ALIGNMENT_H + +//-- includes ----- +#include "psmove_glm_math.hpp" + +//-- pre-declarations ----- + +//-- constants ---- + +//-- macros ---- + +//-- inline ----- + +//-- interface ----- +void +psmove_alignment_compute_objective_vector( + const glm::quat &q, const glm::vec3 &d, const glm::vec3 &s, + glm::vec3 &out_f, float *out_squared_error); + +void +psmove_alignment_compute_objective_jacobian( + const glm::quat &q, const glm::vec3 &d, glm::mat3x4 &J); + +bool +psmove_alignment_quaternion_between_vector_frames( + const glm::vec3* from[2], const glm::vec3* to[2], const float tolerance, const glm::quat &initial_q, + glm::quat &out_q); + +#endif // __PSMOVE_ALIGNMENT_H \ No newline at end of file diff --git a/src/math/psmove_glm_math.hpp b/src/math/psmove_glm_math.hpp new file mode 100644 index 00000000..147f2a27 --- /dev/null +++ b/src/math/psmove_glm_math.hpp @@ -0,0 +1,45 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +/* General math utility functions */ +#ifndef __PSMOVE_GLM_MATH_H +#define __PSMOVE_GLM_MATH_H + +//-- includes ----- +#include +#include +#include +#include + +#include "psmove_math.h" + +//-- macros ----- +#define assert_vec3f_is_normalized(v) assert(is_nearly_equal(glm::dot(v,v), 1.f, k_normal_epsilon)) + + +#endif // __PSMOVE_GLM_MATH_H \ No newline at end of file diff --git a/src/math/psmove_math.c b/src/math/psmove_math.c new file mode 100644 index 00000000..84c53ff5 --- /dev/null +++ b/src/math/psmove_math.c @@ -0,0 +1,61 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +//-- includes ----- +#include "psmove_math.h" + +//-- public methods ----- +float clampf(float x, float lo, float hi) +{ + return fminf(fmaxf(x, lo), hi); +} + +float clampf01(float x) +{ + return clampf(x, 0.f, 1.f); +} + +float lerpf(float a, float b, float u) +{ + return a*(1.f - u) + b*u; +} + +float lerp_clampf(float a, float b, float u) +{ + return clampf(lerpf(a, b, u), a, b); +} + +float degrees_to_radians(float x) +{ + return ((x * k_real_pi) / 180.f); +} + +float radians_to_degrees(float x) +{ + return ((x * 180.f) / k_real_pi); +} \ No newline at end of file diff --git a/src/math/psmove_math.h b/src/math/psmove_math.h new file mode 100644 index 00000000..12f2b986 --- /dev/null +++ b/src/math/psmove_math.h @@ -0,0 +1,93 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +/* General math utility functions */ +#ifndef __PSMOVE_MATH_H +#define __PSMOVE_MATH_H + +//-- includes ----- +#include +#include + +//-- constants ---- +#define k_real_max FLT_MAX +#define k_real_min FLT_MIN + +#define k_positional_epsilon 0.001f +#define k_normal_epsilon 0.0001f +#define k_real_epsilon FLT_EPSILON + +#define k_real_pi (float)M_PI +#define k_real_two_pi 2.f*k_real_pi +#define k_real_half_pi 0.5f*k_real_pi + +//-- macros ---- +#ifdef isfinite +#define is_valid_float(x) (!isnan(x) && isfinite(x)) +#else +#define is_valid_float(x) (!isnan(x)) +#endif + +#define is_nearly_equal(a, b, epsilon) (fabsf(a-b) <= epsilon) +#define is_nearly_zero(x) is_nearly_equal(x, 0.0f, k_real_epsilon) + +#define safe_divide_with_default(numerator, denomenator, default_result) (is_nearly_zero(denomenator) ? (default_result) : (numerator / denomenator)); + +#ifndef sgn +#define sgn(x) (((x) >= 0) ? 1 : -1) +#endif + +#ifndef sqr +#define sqr(x) (x*x) +#endif + +#ifdef NDEBUG +#define assert_valid_float(x) assert(is_valid_float(x)) +#else +#define assert_valid_float(x) ((void)0) +#endif + +//-- inline ----- +#ifdef __cplusplus +extern "C" { +#endif + +float clampf(float x, float lo, float hi); +float clampf01(float x); + +float lerpf(float a, float b, float u); +float lerp_clampf(float a, float b, float u); + +float degrees_to_radians(float x); +float radians_to_degrees(float x); + +#ifdef __cplusplus +} +#endif + +#endif // __PSMOVE_MATH_H \ No newline at end of file diff --git a/src/math/psmove_quaternion.cpp b/src/math/psmove_quaternion.cpp new file mode 100644 index 00000000..aa577631 --- /dev/null +++ b/src/math/psmove_quaternion.cpp @@ -0,0 +1,92 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +//-- includes ---- +#include "psmove_quaternion.hpp" + +#include + +//-- globals ---- +const glm::quat g_psmove_quaternion_zero = glm::quat(0, 0, 0, 0); +const glm::quat *k_psmove_quaternion_zero = &g_psmove_quaternion_zero; + +const glm::quat g_psmove_quaternion_identity = glm::quat(1, 0, 0, 0); +const glm::quat *k_psmove_quaternion_identity = &g_psmove_quaternion_identity; + +//-- public methods ---- + +glm::quat +psmove_quaternion_safe_divide_with_default(const glm::quat &q, const float divisor, const glm::quat &default_result) +{ + glm::quat q_n; + + if (!is_nearly_zero(divisor)) + { + q_n = q / divisor; + } + else + { + q_n = default_result; + } + + return q_n; +} + +glm::quat +psmove_quaternion_normalized_lerp(const glm::quat &a, const glm::quat &b, const float u) +{ + glm::quat q= a*(1.f - u) + b*u; + + psmove_quaternion_normalize_with_default(q, g_psmove_quaternion_identity); + + return q; +} + +float +psmove_quaternion_normalize_with_default(glm::quat &inout_v, const glm::quat &default_result) +{ + const float magnitude = glm::length(inout_v); + inout_v = psmove_quaternion_safe_divide_with_default(inout_v, magnitude, default_result); + return magnitude; +} + +bool +psmove_quaternion_is_valid(const glm::quat &q) +{ + return is_valid_float(q.x) && is_valid_float(q.y) && is_valid_float(q.z) && is_valid_float(q.w); +} + +glm::vec3 +psmove_vector3f_clockwise_rotate(const glm::quat &q, const glm::vec3 &v) +{ + assert_quaternion_is_normalized(q); + + // GLM rotates counterclockwise (i.e. q*v*q^-1), + // while we want the inverse of that (q^-1*v*q) + return glm::conjugate(q) * v; +} \ No newline at end of file diff --git a/src/math/psmove_quaternion.hpp b/src/math/psmove_quaternion.hpp new file mode 100644 index 00000000..75ad55d8 --- /dev/null +++ b/src/math/psmove_quaternion.hpp @@ -0,0 +1,66 @@ + +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +/* Math functions related to quaternions */ +#ifndef __PSMOVE_QUATERNION_H +#define __PSMOVE_QUATERNION_H + +//-- includes ----- +#include "psmove_glm_math.hpp" +#include "psmove_vector.h" + +//-- pre-declarations ---- + +//-- structures ----- + +//-- constants ----- +extern const glm::quat *k_psmove_quaternion_zero; +extern const glm::quat *k_psmove_quaternion_identity; + +//-- interface ----- + +glm::quat +psmove_quaternion_safe_divide_with_default(const glm::quat &q, const float divisor, const glm::quat &default_result); + +glm::quat +psmove_quaternion_normalized_lerp(const glm::quat &a, const glm::quat &b, const float u); + +float +psmove_quaternion_normalize_with_default(glm::quat &inout_v, const glm::quat &default_result); + +bool +psmove_quaternion_is_valid(const glm::quat &q); + +glm::vec3 +psmove_vector3f_clockwise_rotate(const glm::quat &q, const glm::vec3 &v); + +//-- macros ----- +#define assert_quaternion_is_normalized(q) assert(is_nearly_equal(glm::dot(q,q), 1.f, k_normal_epsilon)) + +#endif // __PSMOVE_QUATERNION_H \ No newline at end of file diff --git a/src/math/psmove_vector.c b/src/math/psmove_vector.c new file mode 100644 index 00000000..124835e5 --- /dev/null +++ b/src/math/psmove_vector.c @@ -0,0 +1,223 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +//-- includes ---- +#include "psmove.h" +#include "psmove_math.h" +#include "psmove_vector.h" +#include +#include + +//-- globals ---- +const PSMove_3AxisVector g_psmove_vector_zero = {0, 0, 0}; +const PSMove_3AxisVector *k_psmove_vector_zero = &g_psmove_vector_zero; + +const PSMove_3AxisVector g_psmove_vector_one = {1, 1, 1}; +const PSMove_3AxisVector *k_psmove_vector_one = &g_psmove_vector_one; + +const PSMove_3AxisVector g_psmove_vector_i = {1, 0, 0}; +const PSMove_3AxisVector *k_psmove_vector_i = &g_psmove_vector_i; + +const PSMove_3AxisVector g_psmove_vector_j = {0, 1, 0}; +const PSMove_3AxisVector *k_psmove_vector_j = &g_psmove_vector_j; + +const PSMove_3AxisVector g_psmove_vector_k = {0, 0, 1}; +const PSMove_3AxisVector *k_psmove_vector_k = &g_psmove_vector_k; + +//-- public methods ---- +PSMove_3AxisVector +psmove_3axisvector_xyz(const float x, const float y, const float z) +{ + PSMove_3AxisVector v = { x, y, z }; + + return v; +} + +PSMove_3AxisVector +psmove_3axisvector_add(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + PSMove_3AxisVector v; + + v.x = a->x + b->x; + v.y = a->y + b->y; + v.z = a->z + b->z; + + return v; +} + +PSMove_3AxisVector +psmove_3axisvector_subtract(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + PSMove_3AxisVector v; + + v.x = a->x - b->x; + v.y = a->y - b->y; + v.z = a->z - b->z; + + return v; +} + +PSMove_3AxisVector +psmove_3axisvector_scale(const PSMove_3AxisVector *v, const float s) +{ + PSMove_3AxisVector w; + + w.x = v->x * s; + w.y = v->y * s; + w.z = v->z * s; + + return w; +} + +PSMove_3AxisVector +psmove_3axisvector_divide_by_scalar_unsafe(const PSMove_3AxisVector *v, const float divisor) +{ + PSMove_3AxisVector result; + + result.x = v->x / divisor; + result.y = v->y / divisor; + result.z = v->z / divisor; + + return result; +} + +PSMove_3AxisVector +psmove_3axisvector_divide_by_vector_unsafe(const PSMove_3AxisVector *v, const PSMove_3AxisVector *divisor) +{ + PSMove_3AxisVector result; + + result.x = v->x / divisor->x; + result.y = v->y / divisor->y; + result.z = v->z / divisor->z; + + return result; +} + +PSMove_3AxisVector +psmove_3axisvector_divide_by_scalar_with_default(const PSMove_3AxisVector *v, const float divisor, const PSMove_3AxisVector *default_result) +{ + PSMove_3AxisVector w; + + if (!is_nearly_zero(divisor)) + { + w.x = v->x / divisor; + w.y = v->y / divisor; + w.z = v->z / divisor; + } + else + { + w = *default_result; + } + + return w; +} + +PSMove_3AxisVector +psmove_3axisvector_divide_by_vector_with_default(const PSMove_3AxisVector *v, const PSMove_3AxisVector *divisor, const PSMove_3AxisVector *default_result) +{ + PSMove_3AxisVector result; + + result.x = safe_divide_with_default(v->x, divisor->x, default_result->x); + result.y = safe_divide_with_default(v->y, divisor->y, default_result->y); + result.z = safe_divide_with_default(v->z, divisor->z, default_result->z); + + return result; +} + +float +psmove_3axisvector_dot(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + return (a->x*b->x + a->y*b->y + a->z*b->z); +} + +float +psmove_3axisvector_min_component(const PSMove_3AxisVector *v) +{ + return (float)fmin((float)fmin(v->x, v->y), v->z); +} + +float +psmove_3axisvector_max_component(const PSMove_3AxisVector *v) +{ + return (float)fmax((float)fmax(v->x, v->y), v->z); +} + +PSMove_3AxisVector +psmove_3axisvector_min_vector(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + return psmove_3axisvector_xyz((float)fmin(a->x, b->x), (float)fmin(a->y, b->y), (float)fmin(a->z, b->z)); +} + +PSMove_3AxisVector +psmove_3axisvector_max_vector(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + return psmove_3axisvector_xyz((float)fmax(a->x, b->x), (float)fmax(a->y, b->y), (float)fmax(a->z, b->z)); +} + +float +psmove_3axisvector_length_squared(const PSMove_3AxisVector *v) +{ + return psmove_3axisvector_dot(v,v); +} + +float +psmove_3axisvector_length(const PSMove_3AxisVector *v) +{ + return sqrtf(psmove_3axisvector_dot(v, v)); +} + +float +psmove_3axisvector_normalize_with_default(PSMove_3AxisVector *inout_v, const PSMove_3AxisVector *default_result) +{ + const float length = psmove_3axisvector_length(inout_v); + + *inout_v = psmove_3axisvector_divide_by_scalar_with_default(inout_v, length, default_result); + + return length; +} + +float +psmove_radians_between_vectors(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b) +{ + float divisor = psmove_3axisvector_length(a)*psmove_3axisvector_length(b); + assert(false); // no unit test + + return !is_nearly_zero(divisor) ? acosf(psmove_3axisvector_dot(a, b) / divisor) : 0.f; +} + +PSMove_3AxisVector +psmove_3axisvector_apply_transform(const PSMove_3AxisVector *v, const PSMove_3AxisTransform *m) +{ + PSMove_3AxisVector result = + psmove_3axisvector_xyz( + m->row0[0]*v->x + m->row0[1]*v->y + m->row0[2]*v->z, + m->row1[0]*v->x + m->row1[1]*v->y + m->row1[2]*v->z, + m->row2[0]*v->x + m->row2[1]*v->y + m->row2[2]*v->z); + + return result; +} \ No newline at end of file diff --git a/src/math/psmove_vector.h b/src/math/psmove_vector.h new file mode 100644 index 00000000..f0936aab --- /dev/null +++ b/src/math/psmove_vector.h @@ -0,0 +1,109 @@ +/** +* PS Move API - An interface for the PS Move Motion Controller +* Copyright (c) 2011, 2012 Thomas Perl +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +**/ + +/* Math functions related to 3d vectors */ +#ifndef __PSMOVE_VECTOR_H +#define __PSMOVE_VECTOR_H + +//-- includes ----- +#include +#include + +#include "psmove_math.h" +#include "psmove.h" + +//-- interface ----- +#ifdef __cplusplus +extern "C" { +#endif +ADDAPI extern const PSMove_3AxisVector *k_psmove_vector_zero; +ADDAPI extern const PSMove_3AxisVector *k_psmove_vector_one; +ADDAPI extern const PSMove_3AxisVector *k_psmove_vector_i; +ADDAPI extern const PSMove_3AxisVector *k_psmove_vector_j; +ADDAPI extern const PSMove_3AxisVector *k_psmove_vector_k; + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_xyz(const float x, const float y, const float z); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_add(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_subtract(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_scale(const PSMove_3AxisVector *v, const float s); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_divide_by_scalar_unsafe(const PSMove_3AxisVector *v, const float divisor); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_divide_by_vector_unsafe(const PSMove_3AxisVector *v, const PSMove_3AxisVector *divisor); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_divide_by_scalar_with_default(const PSMove_3AxisVector *v, const float divisor, const PSMove_3AxisVector *default_result); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_divide_by_vector_with_default(const PSMove_3AxisVector *v, const PSMove_3AxisVector *divisor, const PSMove_3AxisVector *default_result); + +ADDAPI float +ADDCALL psmove_3axisvector_dot(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b); + +ADDAPI float +ADDCALL psmove_3axisvector_min_component(const PSMove_3AxisVector *v); + +ADDAPI float +ADDCALL psmove_3axisvector_max_component(const PSMove_3AxisVector *v); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_min_vector(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_max_vector(const PSMove_3AxisVector *a, const PSMove_3AxisVector *b); + +ADDAPI float +ADDCALL psmove_3axisvector_length_squared(const PSMove_3AxisVector *v); + +ADDAPI float +ADDCALL psmove_3axisvector_length(const PSMove_3AxisVector *v); + +ADDAPI float +ADDCALL psmove_3axisvector_normalize_with_default(PSMove_3AxisVector *inout_v, const PSMove_3AxisVector *default_result); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_3axisvector_apply_transform(const PSMove_3AxisVector *v, const PSMove_3AxisTransform *m); + +#ifdef __cplusplus +} +#endif + +//-- macros ----- +#define assert_vector_is_normalized(v) assert(is_nearly_equal(psmove_vector_length_squared(v), 1.f, k_normal_epsilon)) +#define assert_vectors_are_perpendicular(a, b) assert(is_nearly_equal(psmove_vector_dot(a,b), 0.f, k_normal_epsilon)) + +#endif // __PSMOVE_VECTOR_H \ No newline at end of file diff --git a/src/psmove.c b/src/psmove.c index 0206d929..fb59f69f 100644 --- a/src/psmove.c +++ b/src/psmove.c @@ -31,6 +31,7 @@ #include "psmove_private.h" #include "psmove_calibration.h" #include "psmove_orientation.h" +#include "math/psmove_vector.h" #include #include @@ -231,36 +232,6 @@ typedef struct { unsigned char extdata[PSMOVE_EXT_DATA_BUF_SIZE]; /* external device data (EXT port) */ } PSMove_Data_Input; -typedef struct { - int x; - int y; - int z; -} PSMove_3AxisVector; - -int -psmove_3axisvector_min(PSMove_3AxisVector vector) -{ - if (vector.x < vector.y && vector.x < vector.z) { - return vector.x; - } else if (vector.y < vector.z) { - return vector.y; - } else { - return vector.z; - } -} - -int -psmove_3axisvector_max(PSMove_3AxisVector vector) -{ - if (vector.x > vector.y && vector.x > vector.z) { - return vector.x; - } else if (vector.y > vector.z) { - return vector.y; - } else { - return vector.z; - } -} - struct _PSMove { /* Device type (hidapi-based or moved-based */ enum PSMove_Device_Type type; @@ -307,6 +278,9 @@ struct _PSMove { /* Is orientation tracking currently enabled? */ enum PSMove_Bool orientation_enabled; + /* The direction of the magnetic field found during calibration */ + PSMove_3AxisVector magnetometer_calibration_direction; + /* Minimum and maximum magnetometer values observed this session */ PSMove_3AxisVector magnetometer_min; PSMove_3AxisVector magnetometer_max; @@ -326,7 +300,7 @@ struct _PSMove { #endif }; -void +enum PSMove_Bool psmove_load_magnetometer_calibration(PSMove *move); /* End private definitions */ @@ -1824,50 +1798,60 @@ psmove_get_gyroscope_frame(PSMove *move, enum PSMove_Frame frame, } void -psmove_get_magnetometer_vector(PSMove *move, - float *mx, float *my, float *mz) +psmove_get_magnetometer_3axisvector(PSMove *move, PSMove_3AxisVector *out_m) { - PSMove_3AxisVector raw; - psmove_return_if_fail(move != NULL); - psmove_get_magnetometer(move, &raw.x, &raw.y, &raw.z); + PSMove_3AxisVector m; + int mx, my, mz; + psmove_get_magnetometer(move, &mx, &my, &mz); + m= psmove_3axisvector_xyz((float)mx, (float)my, (float)mz); - /* Update minimum values */ - if (raw.x < move->magnetometer_min.x) { - move->magnetometer_min.x = raw.x; - } - if (raw.y < move->magnetometer_min.y) { - move->magnetometer_min.y = raw.y; - } - if (raw.z < move->magnetometer_min.z) { - move->magnetometer_min.z = raw.z; - } - - /* Update maximum values */ - if (raw.x > move->magnetometer_max.x) { - move->magnetometer_max.x = raw.x; - } - if (raw.y > move->magnetometer_max.y) { - move->magnetometer_max.y = raw.y; - } - if (raw.z > move->magnetometer_max.z) { - move->magnetometer_max.z = raw.z; - } + /* Update minimum and max components */ + move->magnetometer_min = psmove_3axisvector_min_vector(&move->magnetometer_min, &m); + move->magnetometer_max = psmove_3axisvector_max_vector(&move->magnetometer_max, &m); /* Map [min..max] to [-1..+1] */ - if (mx) { - *mx = -1.f + 2.f * (float)(raw.x - move->magnetometer_min.x) / - (float)(move->magnetometer_max.x - move->magnetometer_min.x); - } - if (my) { - *my = -1.f + 2.f * (float)(raw.y - move->magnetometer_min.y) / - (float)(move->magnetometer_max.y - move->magnetometer_min.y); - } - if (mz) { - *mz = -1.f + 2.f * (float)(raw.z - move->magnetometer_min.z) / - (float)(move->magnetometer_max.z - move->magnetometer_min.z); - } + if (out_m) + { + PSMove_3AxisVector range = psmove_3axisvector_subtract(&move->magnetometer_max, &move->magnetometer_min); + PSMove_3AxisVector offset = psmove_3axisvector_subtract(&m, &move->magnetometer_min); + + // 2*(raw-move->magnetometer_min)/(move->magnetometer_max - move->magnetometer_min) - <1,1,1> + *out_m = psmove_3axisvector_divide_by_vector_with_default(&offset, &range, k_psmove_vector_zero); + *out_m = psmove_3axisvector_scale(out_m, 2.f); + *out_m = psmove_3axisvector_subtract(out_m, k_psmove_vector_one); + + // The magnetometer y-axis is flipped compared to the accelerometer and gyro. + // Flip it back around to get it into the same space. + out_m->y = -out_m->y; + } +} + +void +psmove_get_magnetometer_vector(PSMove *move, + float *mx, float *my, float *mz) +{ + psmove_return_if_fail(move != NULL); + + PSMove_3AxisVector m; + + psmove_get_magnetometer_3axisvector(move, &m); + + if (mx) + { + *mx= m.x; + } + + if (my) + { + *my= m.y; + } + + if (mz) + { + *mz= m.z; + } } enum PSMove_Bool @@ -1921,13 +1905,8 @@ psmove_enable_orientation(PSMove *move, enum PSMove_Bool enabled) enum PSMove_Bool psmove_has_orientation(PSMove *move) { - psmove_return_val_if_fail(move != NULL, 0); - psmove_return_val_if_fail(move->orientation != NULL, 0); - -#if !defined(PSMOVE_WITH_MADGWICK_AHRS) - psmove_WARNING("Built without Madgwick AHRS - no orientation support"); - return PSMove_False; -#endif + psmove_return_val_if_fail(move != NULL, PSMove_False); + psmove_return_val_if_fail(move->orientation != NULL, PSMove_False); return move->orientation_enabled; } @@ -1956,12 +1935,13 @@ psmove_reset_magnetometer_calibration(PSMove *move) { psmove_return_if_fail(move != NULL); + move->magnetometer_calibration_direction = *k_psmove_vector_zero; move->magnetometer_min.x = move->magnetometer_min.y = - move->magnetometer_min.z = INT_MAX; + move->magnetometer_min.z = FLT_MAX; move->magnetometer_max.x = move->magnetometer_max.y = - move->magnetometer_max.z = INT_MIN; + move->magnetometer_max.z = FLT_MIN; } char * @@ -1996,75 +1976,241 @@ psmove_save_magnetometer_calibration(PSMove *move) free(filename); psmove_return_if_fail(fp != NULL); + fprintf(fp, "mx,my,mz\n"); + fprintf(fp, "%f,%f,%f\n", + move->magnetometer_calibration_direction.x, + move->magnetometer_calibration_direction.y, + move->magnetometer_calibration_direction.z); fprintf(fp, "axis,min,max\n"); - fprintf(fp, "x,%d,%d\n", move->magnetometer_min.x, move->magnetometer_max.x); - fprintf(fp, "y,%d,%d\n", move->magnetometer_min.y, move->magnetometer_max.y); - fprintf(fp, "z,%d,%d\n", move->magnetometer_min.z, move->magnetometer_max.z); + fprintf(fp, "x,%f,%f\n", move->magnetometer_min.x, move->magnetometer_max.x); + fprintf(fp, "y,%f,%f\n", move->magnetometer_min.y, move->magnetometer_max.y); + fprintf(fp, "z,%f,%f\n", move->magnetometer_min.z, move->magnetometer_max.z); fclose(fp); } -void +enum PSMove_Bool psmove_load_magnetometer_calibration(PSMove *move) { - psmove_return_if_fail(move != NULL); + enum PSMove_Bool success = PSMove_False; + + if (move == NULL) { + return success; + } + psmove_reset_magnetometer_calibration(move); char *filename = psmove_get_magnetometer_calibration_filename(move); FILE *fp = fopen(filename, "r"); free(filename); - if (fp == NULL) { + if (fp == NULL) { char *addr = psmove_get_serial(move); psmove_WARNING("Magnetometer in %s not yet calibrated.\n", addr); free(addr); - return; + goto finish; } - char s_axis[5], s_min[4], s_max[4]; + char s_mx[3], s_my[3], s_mz[3]; + float mx, my, mz; + char s_axis[5], s_min[4], s_max[4]; char c_axis; - int i_min, i_max; + float f_min, f_max; int result; - result = fscanf(fp, "%4s,%3s,%3s\n", s_axis, s_min, s_max); + result = fscanf(fp, "%2s,%2s,%2s\n", s_mx, s_my, s_mz); + psmove_goto_if_fail(result == 3, finish); + psmove_goto_if_fail(strcmp(s_mx, "mx") == 0, finish); + psmove_goto_if_fail(strcmp(s_my, "my") == 0, finish); + psmove_goto_if_fail(strcmp(s_mz, "mz") == 0, finish); + + result = fscanf(fp, "%f,%f,%f\n", &mx, &my, &mz); + psmove_goto_if_fail(result == 3, finish); + move->magnetometer_calibration_direction = psmove_3axisvector_xyz(mx, my, mz); + + result = fscanf(fp, "%4s,%3s,%3s\n", s_axis, s_min, s_max); psmove_goto_if_fail(result == 3, finish); psmove_goto_if_fail(strcmp(s_axis, "axis") == 0, finish); psmove_goto_if_fail(strcmp(s_min, "min") == 0, finish); psmove_goto_if_fail(strcmp(s_max, "max") == 0, finish); - result = fscanf(fp, "%c,%d,%d\n", &c_axis, &i_min, &i_max); + result = fscanf(fp, "%c,%f,%f\n", &c_axis, &f_min, &f_max); psmove_goto_if_fail(result == 3, finish); psmove_goto_if_fail(c_axis == 'x', finish); - move->magnetometer_min.x = i_min; - move->magnetometer_max.x = i_max; + move->magnetometer_min.x = f_min; + move->magnetometer_max.x = f_max; - result = fscanf(fp, "%c,%d,%d\n", &c_axis, &i_min, &i_max); + result = fscanf(fp, "%c,%f,%f\n", &c_axis, &f_min, &f_max); psmove_goto_if_fail(result == 3, finish); psmove_goto_if_fail(c_axis == 'y', finish); - move->magnetometer_min.y = i_min; - move->magnetometer_max.y = i_max; + move->magnetometer_min.y = f_min; + move->magnetometer_max.y = f_max; - result = fscanf(fp, "%c,%d,%d\n", &c_axis, &i_min, &i_max); + result = fscanf(fp, "%c,%f,%f\n", &c_axis, &f_min, &f_max); psmove_goto_if_fail(result == 3, finish); psmove_goto_if_fail(c_axis == 'z', finish); - move->magnetometer_min.z = i_min; - move->magnetometer_max.z = i_max; + move->magnetometer_min.z = f_min; + move->magnetometer_max.z = f_max; + + success = PSMove_True; finish: - fclose(fp); + if (success == PSMove_False) + { + psmove_reset_magnetometer_calibration(move); + } + + if (fp != NULL) + { + fclose(fp); + } + + return success; } -int +float psmove_get_magnetometer_calibration_range(PSMove *move) { psmove_return_val_if_fail(move != NULL, 0); - PSMove_3AxisVector diff = { - move->magnetometer_max.x - move->magnetometer_min.x, - move->magnetometer_max.y - move->magnetometer_min.y, - move->magnetometer_max.z - move->magnetometer_min.z, - }; + PSMove_3AxisVector diff = psmove_3axisvector_subtract(&move->magnetometer_max, &move->magnetometer_min); + + return psmove_3axisvector_min_component(&diff); +} + +void +psmove_set_orientation_fusion_type(PSMove *move, enum PSMoveOrientation_Fusion_Type fusion_type) +{ + psmove_return_if_fail(move != NULL); + psmove_return_if_fail(move->orientation != NULL); + + psmove_orientation_set_fusion_type(move->orientation, fusion_type); +} + +void +psmove_set_calibration_pose(PSMove *move, enum PSMove_CalibrationPose_Type calibration_pose) +{ + switch (calibration_pose) + { + case CalibrationPose_Upright: + psmove_set_calibration_transform(move, k_psmove_identity_pose_upright); + break; + case CalibrationPose_LyingFlat: + psmove_set_calibration_transform(move, k_psmove_identity_pose_laying_flat); + break; + } +} + +void +psmove_set_calibration_transform(PSMove *move, const PSMove_3AxisTransform *transform) +{ + psmove_return_if_fail(move != NULL); + psmove_return_if_fail(move->orientation != NULL); + + psmove_orientation_set_calibration_transform(move->orientation, transform); +} + +void +psmove_get_identity_gravity_calibration_direction(PSMove *move, PSMove_3AxisVector *out_a) +{ + *out_a= psmove_3axisvector_xyz(0.f, 1.f, 0.f); +} + +void +psmove_get_transformed_gravity_calibration_direction(PSMove *move, PSMove_3AxisVector *out_a) +{ + if (move != NULL) + { + *out_a= psmove_orientation_get_gravity_calibration_direction(move->orientation); + } + else + { + psmove_get_identity_gravity_calibration_direction(move, out_a); + } +} + +void +psmove_get_identity_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *out_m) +{ + psmove_return_if_fail(move != NULL); + psmove_return_if_fail(out_m != NULL); + + *out_m= move->magnetometer_calibration_direction; +} + +void +psmove_get_transformed_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *out_m) +{ + if (move != NULL) + { + *out_m= psmove_orientation_get_magnetometer_calibration_direction(move->orientation); + } + else + { + psmove_get_identity_magnetometer_calibration_direction(move, out_m); + } +} + +void +psmove_set_magnetometer_calibration_direction(PSMove *move, PSMove_3AxisVector *m) +{ + psmove_return_if_fail(move != NULL); + + move->magnetometer_calibration_direction = *m; +} + +void +psmove_set_sensor_data_basis(PSMove *move, enum PSMove_SensorDataBasis_Type basis_type) +{ + switch (basis_type) + { + case SensorDataBasis_Native: + psmove_set_sensor_data_transform(move, k_psmove_sensor_transform_identity); + break; + case SensorDataBasis_OpenGL: + psmove_set_sensor_data_transform(move, k_psmove_sensor_transform_opengl); + break; + } +} + +void +psmove_set_sensor_data_transform(PSMove *move, const PSMove_3AxisTransform *transform) +{ + psmove_return_if_fail(move != NULL); + psmove_return_if_fail(move->orientation != NULL); + + psmove_orientation_set_sensor_data_transform(move->orientation, transform); +} + +void +psmove_get_transformed_magnetometer_direction(PSMove *move, PSMove_3AxisVector *out_m) +{ + psmove_return_if_fail(move != NULL); + + *out_m= psmove_orientation_get_magnetometer_normalized_vector(move->orientation); +} + +void +psmove_get_transformed_accelerometer_frame_3axisvector(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_a) +{ + psmove_return_if_fail(move != NULL); + + *out_a= psmove_orientation_get_accelerometer_vector(move->orientation, frame); +} + +void +psmove_get_transformed_accelerometer_frame_direction(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_a) +{ + psmove_return_if_fail(move != NULL); + + *out_a= psmove_orientation_get_accelerometer_normalized_vector(move->orientation, frame); +} + +void +psmove_get_transformed_gyroscope_frame_3axisvector(PSMove *move, enum PSMove_Frame frame, PSMove_3AxisVector *out_w) +{ + psmove_return_if_fail(move != NULL); - return psmove_3axisvector_min(diff); + *out_w= psmove_orientation_get_gyroscope_vector(move->orientation, frame); } void diff --git a/src/psmove_orientation.c b/src/psmove_orientation.c deleted file mode 100644 index a6dce292..00000000 --- a/src/psmove_orientation.c +++ /dev/null @@ -1,255 +0,0 @@ - - /** - * PS Move API - An interface for the PS Move Motion Controller - * Copyright (c) 2012 Thomas Perl - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - **/ - - -#include -#include -#include -#include - -#include "psmove.h" -#include "psmove_private.h" -#include "psmove_orientation.h" - -#if defined(PSMOVE_WITH_MADGWICK_AHRS) -# include "../external/MadgwickAHRS/MadgwickAHRS.h" -#endif - - -struct _PSMoveOrientation { - PSMove *move; - - /* 9 values = 3x accelerometer + 3x gyroscope + 3x magnetometer */ - float output[9]; - - /* Current sampling frequency */ - float sample_freq; - - /* Sample frequency measurements */ - long sample_freq_measure_start; - long sample_freq_measure_count; - - /* Output value as quaternion */ - float quaternion[4]; - - /* Quaternion measured when controller points towards camera */ - float reset_quaternion[4]; -}; - - -PSMoveOrientation * -psmove_orientation_new(PSMove *move) -{ - psmove_return_val_if_fail(move != NULL, NULL); - - if (!psmove_has_calibration(move)) { - psmove_DEBUG("Can't create orientation - no calibration!\n"); - return NULL; - } - - PSMoveOrientation *orientation = calloc(1, sizeof(PSMoveOrientation)); - - orientation->move = move; - - /* Initial sampling frequency */ - orientation->sample_freq = 120.0f; - - /* Measurement */ - orientation->sample_freq_measure_start = psmove_util_get_ticks(); - orientation->sample_freq_measure_count = 0; - - /* Initial quaternion */ - orientation->quaternion[0] = 1.f; - orientation->quaternion[1] = 0.f; - orientation->quaternion[2] = 0.f; - orientation->quaternion[3] = 0.f; - - return orientation; -} - - -void -psmove_orientation_update(PSMoveOrientation *orientation) -{ - psmove_return_if_fail(orientation != NULL); - - int frame; - - long now = psmove_util_get_ticks(); - if (now - orientation->sample_freq_measure_start >= 1000) { - float measured = ((float)orientation->sample_freq_measure_count) / - ((float)(now-orientation->sample_freq_measure_start))*1000.0f; - psmove_DEBUG("Measured sample_freq: %f\n", measured); - - orientation->sample_freq = measured; - orientation->sample_freq_measure_start = now; - orientation->sample_freq_measure_count = 0; - } - - /* We get 2 measurements per call to psmove_poll() */ - orientation->sample_freq_measure_count += 2; - - psmove_get_magnetometer_vector(orientation->move, - &orientation->output[6], - &orientation->output[7], - &orientation->output[8]); - - float q0 = orientation->quaternion[0]; - float q1 = orientation->quaternion[1]; - float q2 = orientation->quaternion[2]; - float q3 = orientation->quaternion[3]; - - for (frame=0; frame<2; frame++) { - psmove_get_accelerometer_frame(orientation->move, frame, - &orientation->output[0], - &orientation->output[1], - &orientation->output[2]); - - psmove_get_gyroscope_frame(orientation->move, frame, - &orientation->output[3], - &orientation->output[4], - &orientation->output[5]); - -#if defined(PSMOVE_WITH_MADGWICK_AHRS) - MadgwickAHRSupdate(orientation->quaternion, - orientation->sample_freq, - - /* Accelerometer */ - orientation->output[0], - orientation->output[2], - -orientation->output[1], - - /* Gyroscope */ - orientation->output[3], - orientation->output[5], - -orientation->output[4], - - /* Magnetometer */ - orientation->output[6], - orientation->output[8], - orientation->output[7] - ); -#else - psmove_DEBUG("Built without Madgwick AHRS - no orientation tracking"); - return; -#endif - - if (isnan(orientation->quaternion[0]) || - isnan(orientation->quaternion[1]) || - isnan(orientation->quaternion[2]) || - isnan(orientation->quaternion[3])) { - psmove_DEBUG("Orientation is NaN!"); - orientation->quaternion[0] = q0; - orientation->quaternion[1] = q1; - orientation->quaternion[2] = q2; - orientation->quaternion[3] = q3; - } - } -} - -void -psmove_orientation_get_quaternion(PSMoveOrientation *orientation, - float *q0, float *q1, float *q2, float *q3) -{ - psmove_return_if_fail(orientation != NULL); - - /* first factor (reset quaternion) */ - float a_s = orientation->reset_quaternion[0]; - float a_x = orientation->reset_quaternion[1]; - float a_y = orientation->reset_quaternion[2]; - float a_z = orientation->reset_quaternion[3]; - - /* second factor (quaternion) */ - float b_s = orientation->quaternion[0]; - float b_x = orientation->quaternion[1]; - float b_y = orientation->quaternion[2]; - float b_z = orientation->quaternion[3]; - - /** - * Quaternion multiplication: - * http://lxr.kde.org/source/qt/src/gui/math3d/qquaternion.h#198 - **/ - float ww = (a_z + a_x) * (b_x + b_y); - float yy = (a_s - a_y) * (b_s + b_z); - float zz = (a_s + a_y) * (b_s - b_z); - float xx = ww + yy + zz; - float qq = .5f * (xx + (a_z - a_x) * (b_x - b_y)); - - /* Result */ - float r_s = qq - ww + (a_z - a_y) * (b_y - b_z); - float r_x = qq - xx + (a_x + a_s) * (b_x + b_s); - float r_y = qq - yy + (a_s - a_x) * (b_y + b_z); - float r_z = qq - zz + (a_z + a_y) * (b_s - b_x); - - if (q0) { - *q0 = r_s; - } - - if (q1) { - *q1 = r_x; - } - - if (q2) { - *q2 = r_y; - } - - if (q3) { - *q3 = r_z; - } -} - -void -psmove_orientation_reset_quaternion(PSMoveOrientation *orientation) -{ - psmove_return_if_fail(orientation != NULL); - float q0 = orientation->quaternion[0]; - float q1 = orientation->quaternion[1]; - float q2 = orientation->quaternion[2]; - float q3 = orientation->quaternion[3]; - - /** - * Normalize and conjugate in one step: - * - Normalize via the length - * - Conjugate using (scalar, x, y, z) -> (scalar, -x, -y, -z) - **/ - double length = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); - orientation->reset_quaternion[0] = (float)(q0 / length); - orientation->reset_quaternion[1] = (float)(-q1 / length); - orientation->reset_quaternion[2] = (float)(-q2 / length); - orientation->reset_quaternion[3] = (float)(-q3 / length); -} - -void -psmove_orientation_free(PSMoveOrientation *orientation) -{ - psmove_return_if_fail(orientation != NULL); - - free(orientation); -} - diff --git a/src/psmove_orientation.cpp b/src/psmove_orientation.cpp new file mode 100644 index 00000000..6a850f29 --- /dev/null +++ b/src/psmove_orientation.cpp @@ -0,0 +1,690 @@ + + /** + * PS Move API - An interface for the PS Move Motion Controller + * Copyright (c) 2012 Thomas Perl + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + **/ + +//-- includes ----- +#ifndef _MSC_VER +#include +#endif +#include +#include +#include +#include + +#include "psmove.h" +#include "psmove_private.h" +#include "psmove_orientation.h" + +#include "math/psmove_quaternion.hpp" +#include "math/psmove_alignment.hpp" +#include "math/psmove_vector.h" + +#ifdef CMAKE_BUILD +#include +#endif + +//-- constants ----- +#define SAMPLE_FREQUENCY 120.f + +// Madgwick MARG Filter Constants +#define gyroMeasDrift 3.14159265358979f * (0.9f / 180.0f) // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s) +#define beta sqrtf(3.0f / 4.0f) * gyroMeasError // compute beta +#define gyroMeasError 3.14159265358979f * (1.5f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s) +#define zeta sqrtf(3.0f / 4.0f) * gyroMeasDrift // compute zeta + +// Complementary ARG Filter constants +#define k_base_earth_frame_align_weight 0.02f + +const PSMove_3AxisTransform g_psmove_zero_transform = {0,0,0, 0,0,0, 0,0,0}; +const PSMove_3AxisTransform *k_psmove_zero_transform = &g_psmove_zero_transform; + +// Calibration Pose transform +const PSMove_3AxisTransform g_psmove_identity_pose_upright = {1,0,0, 0,1,0, 0,0,1}; +const PSMove_3AxisTransform *k_psmove_identity_pose_upright = &g_psmove_identity_pose_upright; + +const PSMove_3AxisTransform g_psmove_identity_pose_laying_flat = {1,0,0, 0,0,-1, 0,1,0}; +const PSMove_3AxisTransform *k_psmove_identity_pose_laying_flat = &g_psmove_identity_pose_laying_flat; + +//Sensor Transforms +const PSMove_3AxisTransform g_psmove_sensor_transform_identity = {1,0,0, 0,1,0, 0,0,1}; +const PSMove_3AxisTransform *k_psmove_sensor_transform_identity = &g_psmove_sensor_transform_identity; + +const PSMove_3AxisTransform g_psmove_sensor_transform_opengl = {1,0,0, 0,0,1, 0,-1,0}; +const PSMove_3AxisTransform *k_psmove_sensor_transform_opengl= &g_psmove_sensor_transform_opengl; + +//-- structures ---- +struct _PSMoveMadwickMARGState +{ + // estimate gyroscope biases error + glm::quat omega_bias; +}; +typedef struct _PSMoveMadwickMARGState PSMoveMadgwickMARGState; + +struct _PSMovComplementaryMARGState +{ + float mg_weight; +}; +typedef struct _PSMovComplementaryMARGState PSMoveComplementaryMARGState; + +struct _PSMoveOrientation { + PSMove *move; + + /* Current sampling frequency */ + float sample_freq; + + /* Sample frequency measurements */ + long sample_freq_measure_start; + long sample_freq_measure_count; + + /* Output value as quaternion */ + glm::quat quaternion; + + /* Quaternion measured when controller points towards camera */ + glm::quat reset_quaternion; + + /* Transforms the gravity and magnetometer calibration vectors recorded when the + controller was help upright during calibration. This is needed if you want the "identity pose" + to be something besides the one used during calibration */ + PSMove_3AxisTransform calibration_transform; + + /* Transforms the sensor data from PSMove Space to some user defined coordinate space */ + PSMove_3AxisTransform sensor_transform; + + /* Per filter type data */ + enum PSMoveOrientation_Fusion_Type fusion_type; + struct + { + PSMoveMadgwickMARGState madgwick_marg_state; + PSMoveComplementaryMARGState complementary_marg_state; + } fusion_state; +}; + +//-- prototypes ----- +static void _psmove_orientation_fusion_imu_update( + PSMoveOrientation *orientation_state, + float deltat, + const glm::vec3 &sensor_gyroscope, + const glm::vec3 &sensor_accelerometer); +static void _psmove_orientation_fusion_madgwick_marg_update( + PSMoveOrientation *orientation_state, + float deltat, + const glm::vec3 &sensor_gyroscope, + const glm::vec3 &sensor_accelerometer, + const glm::vec3 &sensor_magnetometer); +static void _psmove_orientation_fusion_complementary_marg_update( + PSMoveOrientation *orientation_state, + float delta_t, + const glm::vec3 &sensor_gyroscope, + const glm::vec3 &sensor_acceleration, + const glm::vec3 &sensor_magnetometer); + +//-- public methods ----- +PSMoveOrientation * +psmove_orientation_new(PSMove *move) +{ + psmove_return_val_if_fail(move != NULL, NULL); + + if (!psmove_has_calibration(move)) { + psmove_DEBUG("Can't create orientation - no calibration!\n"); + return NULL; + } + + PSMoveOrientation *orientation_state = (PSMoveOrientation *)calloc(1, sizeof(PSMoveOrientation)); + + orientation_state->move = move; + + /* Initial sampling frequency */ + orientation_state->sample_freq = SAMPLE_FREQUENCY; + + /* Measurement */ + orientation_state->sample_freq_measure_start = psmove_util_get_ticks(); + orientation_state->sample_freq_measure_count = 0; + + /* Initial quaternion */ + orientation_state->quaternion = *k_psmove_quaternion_identity; + orientation_state->reset_quaternion = *k_psmove_quaternion_identity; + + /* Initialize data specific to the selected filter */ + psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_ComplementaryMARG); + + /* Set the transform used re-orient the calibration data used by the orientation fusion algorithm */ + psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_laying_flat); + + /* Set the transform used re-orient the sensor data used by the orientation fusion algorithm */ + psmove_orientation_set_sensor_data_transform(orientation_state, k_psmove_sensor_transform_opengl); + + return orientation_state; +} + +void +psmove_orientation_set_fusion_type(PSMoveOrientation *orientation_state, enum PSMoveOrientation_Fusion_Type fusion_type) +{ + orientation_state->fusion_type = fusion_type; + + switch (fusion_type) + { + case OrientationFusion_None: + case OrientationFusion_MadgwickIMU: + // No initialization + break; + case OrientationFusion_MadgwickMARG: + { + PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state; + + marg_state->omega_bias = *k_psmove_quaternion_zero; + } + break; + case OrientationFusion_ComplementaryMARG: + { + PSMoveComplementaryMARGState *marg_state = &orientation_state->fusion_state.complementary_marg_state; + + // Start off fully using the rotation from earth-frame. + // Then drop down + marg_state->mg_weight = 1.f; + } + break; + default: + break; + } +} + +void +psmove_orientation_set_calibration_transform(PSMoveOrientation *orientation_state, const PSMove_3AxisTransform *transform) +{ + psmove_return_if_fail(orientation_state != NULL); + psmove_return_if_fail(transform != NULL); + + orientation_state->calibration_transform= *transform; +} + +void +psmove_orientation_set_sensor_data_transform(PSMoveOrientation *orientation_state, const PSMove_3AxisTransform *transform) +{ + psmove_return_if_fail(orientation_state != NULL); + psmove_return_if_fail(transform != NULL); + + orientation_state->sensor_transform= *transform; +} + +PSMove_3AxisVector +psmove_orientation_get_gravity_calibration_direction(PSMoveOrientation *orientation_state) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + PSMove_3AxisVector identity_g; + psmove_get_identity_gravity_calibration_direction(orientation_state->move, &identity_g); + + // First apply the calibration data transform. + // This allows us to pretend the "identity pose" was some other orientation the vertical during calibration + identity_g= psmove_3axisvector_apply_transform(&identity_g, &orientation_state->calibration_transform); + + // Next apply the sensor data transform. + // This allows us to pretend the sensors are in some other coordinate system (like OpenGL where +Y is up) + identity_g= psmove_3axisvector_apply_transform(&identity_g, &orientation_state->sensor_transform); + + return identity_g; +} + +PSMove_3AxisVector +psmove_orientation_get_magnetometer_calibration_direction(PSMoveOrientation *orientation_state) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + PSMove_3AxisVector identity_m; + psmove_get_identity_magnetometer_calibration_direction(orientation_state->move, &identity_m); + + // First apply the calibration data transform. + // This allows us to pretend the "identity pose" was some other orientation the vertical during calibration + identity_m= psmove_3axisvector_apply_transform(&identity_m, &orientation_state->calibration_transform); + + // Next apply the sensor data transform. + // This allows us to pretend the sensors are in some other coordinate system (like OpenGL where +Y is up) + identity_m= psmove_3axisvector_apply_transform(&identity_m, &orientation_state->sensor_transform); + + return identity_m; +} + +PSMove_3AxisVector +psmove_orientation_get_accelerometer_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + float ax, ay, az; + psmove_get_accelerometer_frame(orientation_state->move, frame, &ax, &ay, &az); + + // Apply the "identity pose" transform + PSMove_3AxisVector a = psmove_3axisvector_xyz(ax, ay, az); + a= psmove_3axisvector_apply_transform(&a, &orientation_state->sensor_transform); + + return a; +} + +PSMove_3AxisVector +psmove_orientation_get_accelerometer_normalized_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + PSMove_3AxisVector a= psmove_orientation_get_accelerometer_vector(orientation_state, frame); + + // Normalize the accelerometer vector + psmove_3axisvector_normalize_with_default(&a, k_psmove_vector_zero); + + return a; +} + +PSMove_3AxisVector +psmove_orientation_get_gyroscope_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + float omega_x, omega_y, omega_z; + psmove_get_gyroscope_frame(orientation_state->move, frame, &omega_x, &omega_y, &omega_z); + + // Apply the "identity pose" transform + PSMove_3AxisVector omega = psmove_3axisvector_xyz(omega_x, omega_y, omega_z); + omega= psmove_3axisvector_apply_transform(&omega, &orientation_state->sensor_transform); + + return omega; +} + +PSMove_3AxisVector +psmove_orientation_get_magnetometer_normalized_vector(PSMoveOrientation *orientation_state) +{ + psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero); + + PSMove_3AxisVector m; + psmove_get_magnetometer_3axisvector(orientation_state->move, &m); + m= psmove_3axisvector_apply_transform(&m, &orientation_state->sensor_transform); + + // Make sure we always return a normalized direction + psmove_3axisvector_normalize_with_default(&m, k_psmove_vector_zero); + + return m; +} + +void +psmove_orientation_update(PSMoveOrientation *orientation_state) +{ + psmove_return_if_fail(orientation_state != NULL); + + int frame_half; + long now = psmove_util_get_ticks(); + + if (now - orientation_state->sample_freq_measure_start >= 1000) + { + float measured = ((float)orientation_state->sample_freq_measure_count) / + ((float)(now-orientation_state->sample_freq_measure_start))*1000.f; + psmove_DEBUG("Measured sample_freq: %f\n", measured); + + orientation_state->sample_freq = measured; + orientation_state->sample_freq_measure_start = now; + orientation_state->sample_freq_measure_count = 0; + } + + /* We get 2 measurements per call to psmove_poll() */ + orientation_state->sample_freq_measure_count += 2; + + glm::quat quaternion_backup = orientation_state->quaternion; + float deltaT = 1.f / fmax(orientation_state->sample_freq, SAMPLE_FREQUENCY); // time delta = 1/frequency + + for (frame_half=0; frame_half<2; frame_half++) + { + switch (orientation_state->fusion_type) + { + case OrientationFusion_None: + break; + case OrientationFusion_MadgwickIMU: + { + // Get the sensor data transformed by the sensor_transform + PSMove_3AxisVector a= + psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + PSMove_3AxisVector omega= + psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + + // Apply the filter + _psmove_orientation_fusion_imu_update( + orientation_state, + deltaT, + /* Gyroscope */ + glm::vec3(omega.x, omega.y, omega.z), + /* Accelerometer */ + glm::vec3(a.x, a.y, a.z)); + } + break; + case OrientationFusion_MadgwickMARG: + { + PSMove_3AxisVector m = psmove_orientation_get_magnetometer_normalized_vector(orientation_state); + PSMove_3AxisVector a = psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + PSMove_3AxisVector omega = psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + _psmove_orientation_fusion_madgwick_marg_update(orientation_state, deltaT, + /* Gyroscope */ + glm::vec3(omega.x, omega.y, omega.z), + /* Accelerometer */ + glm::vec3(a.x, a.y, a.z), + /* Magnetometer */ + glm::vec3(m.x, m.y, m.z)); + } + break; + case OrientationFusion_ComplementaryMARG: + { + PSMove_3AxisVector m= + psmove_orientation_get_magnetometer_normalized_vector(orientation_state); + PSMove_3AxisVector a= + psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + PSMove_3AxisVector omega= + psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half)); + + // Apply the filter + _psmove_orientation_fusion_complementary_marg_update( + orientation_state, + deltaT, + /* Gyroscope */ + glm::vec3(omega.x, omega.y, omega.z), + /* Accelerometer */ + glm::vec3(a.x, a.y, a.z), + /* Magnetometer */ + glm::vec3(m.x, m.y, m.z)); + } + break; + } + + if (!psmove_quaternion_is_valid(orientation_state->quaternion)) + { + psmove_DEBUG("Orientation is NaN!"); + orientation_state->quaternion = quaternion_backup; + } + } +} + +void +psmove_orientation_get_quaternion(PSMoveOrientation *orientation_state, + float *q0, float *q1, float *q2, float *q3) +{ + psmove_return_if_fail(orientation_state != NULL); + + const glm::quat &reset_quaternion = orientation_state->reset_quaternion; + const glm::quat ¤t_quaternion = orientation_state->quaternion; + glm::quat result= reset_quaternion * current_quaternion; + + if (q0) { + *q0 = result.w; + } + + if (q1) { + *q1 = result.x; + } + + if (q2) { + *q2 = result.y; + } + + if (q3) { + *q3 = result.z; + } +} + +void +psmove_orientation_reset_quaternion(PSMoveOrientation *orientation_state) +{ + psmove_return_if_fail(orientation_state != NULL); + + glm::quat q_inverse = glm::conjugate(orientation_state->quaternion); + + psmove_quaternion_normalize_with_default(q_inverse, *k_psmove_quaternion_identity); + orientation_state->reset_quaternion= q_inverse; +} + +void +psmove_orientation_free(PSMoveOrientation *orientation_state) +{ + psmove_return_if_fail(orientation_state != NULL); + + free(orientation_state); +} + +// -- Orientation Filters ---- + +// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: +// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" +// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf +static void _psmove_orientation_fusion_imu_update( + PSMoveOrientation *orientation_state, + float delta_t, + const glm::vec3 ¤t_omega, + const glm::vec3 ¤t_g) +{ + // Current orientation from earth frame to sensor frame + glm::quat SEq = orientation_state->quaternion; + glm::quat SEq_new = SEq; + + // Compute the quaternion derivative measured by gyroscopes + // Eqn 12) q_dot = 0.5*q*omega + glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); + glm::quat SEqDot_omega = (SEq * 0.5f) *omega; + + if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon)) + { + // Get the direction of the gravitational fields in the identity pose + PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state); + glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z); + + // Eqn 15) Applied to the gravity vector + // Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g| + glm::vec3 f_g; + psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); + + // Eqn 21) Applied to the gravity vector + // Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g] + glm::mat3x4 J_g; + psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); + + // Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa) + // Compute the gradient of the objective function + glm::vec4 gradient_f = J_g * f_g; + glm::quat SEqHatDot = + glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); + + // normalize the gradient + psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); + + // Compute the estimated quaternion rate of change + // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot + glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta); + + // Compute then integrate the estimated quaternion rate + // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t + SEq_new = SEq + SEqDot_est*delta_t; + } + else + { + SEq_new = SEq + SEqDot_omega*delta_t; + } + + // Make sure the net quaternion is a pure rotation quaternion + SEq_new= glm::normalize(SEq_new); + + // Save the new quaternion back into the orientation state + orientation_state->quaternion= SEq_new; +} + +// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper: +// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" +// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf +static void +_psmove_orientation_fusion_madgwick_marg_update( + PSMoveOrientation *orientation_state, + float delta_t, + const glm::vec3 ¤t_omega, + const glm::vec3 ¤t_g, + const glm::vec3 ¤t_m) +{ + // If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update + if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon) || + is_nearly_equal(glm::dot(current_m, current_m), 0.f, k_normal_epsilon*k_normal_epsilon)) + { + _psmove_orientation_fusion_imu_update( + orientation_state, + delta_t, + current_omega, + current_g); + return; + } + + PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state; + + // Current orientation from earth frame to sensor frame + glm::quat SEq = orientation_state->quaternion; + + // Get the direction of the magnetic fields in the identity pose. + // NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46) + // but since we've already done the work in calibration to get this vector, let's just use it. + // This also removes the last assumption in this function about what + // the orientation of the identity-pose is (handled by the sensor transform). + PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state); + glm::vec3 k_identity_m_direction = glm::vec3(identity_m.x, identity_m.y, identity_m.z); + + // Get the direction of the gravitational fields in the identity pose + PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state); + glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z); + + // Eqn 15) Applied to the gravity and magnetometer vectors + // Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g| + // |f_b| + glm::vec3 f_g; + psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL); + + glm::vec3 f_m; + psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL); + + // Eqn 21) Applied to the gravity and magnetometer vectors + // Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b] + glm::mat3x4 J_g; + psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g); + + glm::mat3x4 J_m; + psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m); + + // Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm) + // Compute the gradient of the objective function + glm::vec4 gradient_f = J_g*f_g + J_m*f_m; + glm::quat SEqHatDot = + glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]); + + // normalize the gradient to estimate direction of the gyroscope error + psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero); + + // Eqn 47) omega_err= 2*SEq*SEqHatDot + // compute angular estimated direction of the gyroscope error + glm::quat omega_err = (SEq*2.f) * SEqHatDot; + + // Eqn 48) net_omega_bias+= zeta*omega_err + // Compute the net accumulated gyroscope bias + glm::quat omega_bias= marg_state->omega_bias; + omega_bias = omega_bias + omega_err*zeta*delta_t; + omega_bias.w = 0.f; // no bias should accumulate on the w-component + marg_state->omega_bias= omega_bias; + + // Eqn 49) omega_corrected = omega - net_omega_bias + glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); + glm::quat corrected_omega = omega + (-omega_bias); + + // Compute the rate of change of the orientation purely from the gyroscope + // Eqn 12) q_dot = 0.5*q*omega + glm::quat SEqDot_omega = (SEq * 0.5f) * corrected_omega; + + // Compute the estimated quaternion rate of change + // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot + glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta); + + // Compute then integrate the estimated quaternion rate + // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t + glm::quat SEq_new = SEq + SEqDot_est*delta_t; + + // Make sure the net quaternion is a pure rotation quaternion + SEq_new= glm::normalize(SEq_new); + + // Save the new quaternion back into the orientation state + orientation_state->quaternion= SEq_new; +} + +static void +_psmove_orientation_fusion_complementary_marg_update( + PSMoveOrientation *orientation_state, + float delta_t, + const glm::vec3 ¤t_omega, + const glm::vec3 ¤t_g, + const glm::vec3 ¤t_m) +{ + // Get the direction of the magnetic fields in the identity pose + PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state); + glm::vec3 k_identity_m_direction = glm::vec3(identity_m.x, identity_m.y, identity_m.z); + + // Get the direction of the gravitational field in the identity pose + PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state); + glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z); + + // Angular Rotation (AR) Update + //----------------------------- + // Compute the rate of change of the orientation purely from the gyroscope + // q_dot = 0.5*q*omega + glm::quat q_current= orientation_state->quaternion; + + glm::quat q_omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z); + glm::quat q_derivative = (q_current*0.5f) * q_omega; + + // Integrate the rate of change to get a new orientation + // q_new= q + q_dot*dT + glm::quat q_step = q_derivative * delta_t; + glm::quat ar_orientation = q_current + q_step; + + // Make sure the resulting quaternion is normalized + ar_orientation= glm::normalize(ar_orientation); + + // Magnetic/Gravity (MG) Update + //----------------------------- + const glm::vec3* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction }; + const glm::vec3* mg_to[2] = { ¤t_g, ¤t_m }; + glm::quat mg_orientation; + + // Always attempt to align with the identity_mg, even if we don't get within the alignment tolerance. + // More often then not we'll be better off moving forward with what we have and trying to refine + // the alignment next frame. + psmove_alignment_quaternion_between_vector_frames( + mg_from, mg_to, 0.1f, q_current, mg_orientation); + + // Blending Update + //---------------- + // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame + float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight; + orientation_state->quaternion= + psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight); + + // Update the blend weight + orientation_state->fusion_state.complementary_marg_state.mg_weight = + lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f); +} \ No newline at end of file diff --git a/src/psmove_orientation.h b/src/psmove_orientation.h index b13fda74..150b844f 100644 --- a/src/psmove_orientation.h +++ b/src/psmove_orientation.h @@ -35,29 +35,68 @@ extern "C" { #endif +//-- includes ----- #include "psmove.h" +#include "math/psmove_vector.h" - +//-- pre-declarations ----- struct _PSMoveOrientation; typedef struct _PSMoveOrientation PSMoveOrientation; +//-- constants ----- +extern const PSMove_3AxisTransform *k_psmove_zero_transform; + +/*! Transforms used by psmove_set_orientation_calibration_transform */ +extern const PSMove_3AxisTransform *k_psmove_identity_pose_upright; +extern const PSMove_3AxisTransform *k_psmove_identity_pose_laying_flat; +/*! Transforms used by psmove_set_orientation_sensor_data_transform */ +extern const PSMove_3AxisTransform *k_psmove_sensor_transform_identity; +extern const PSMove_3AxisTransform *k_psmove_sensor_transform_opengl; + +//-- interface ----- ADDAPI PSMoveOrientation * ADDCALL psmove_orientation_new(PSMove *move); ADDAPI void -ADDCALL psmove_orientation_update(PSMoveOrientation *orientation); +ADDCALL psmove_orientation_free(PSMoveOrientation *orientation_state); ADDAPI void -ADDCALL psmove_orientation_get_quaternion(PSMoveOrientation *orientation, - float *q0, float *q1, float *q2, float *q3); +ADDCALL psmove_orientation_set_fusion_type(PSMoveOrientation *orientation_state, enum PSMoveOrientation_Fusion_Type fusion_type); + +ADDAPI void +ADDCALL psmove_orientation_set_calibration_transform(PSMoveOrientation *orientation_state, const PSMove_3AxisTransform *transform); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_gravity_calibration_direction(PSMoveOrientation *orientation_state); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_magnetometer_calibration_direction(PSMoveOrientation *orientation_state); ADDAPI void -ADDCALL psmove_orientation_reset_quaternion(PSMoveOrientation *orientation); +ADDCALL psmove_orientation_set_sensor_data_transform(PSMoveOrientation *orientation_state, const PSMove_3AxisTransform *transform); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_accelerometer_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_accelerometer_normalized_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_gyroscope_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame); + +ADDAPI PSMove_3AxisVector +ADDCALL psmove_orientation_get_magnetometer_normalized_vector(PSMoveOrientation *orientation_state); ADDAPI void -ADDCALL psmove_orientation_free(PSMoveOrientation *orientation); +ADDCALL psmove_orientation_update(PSMoveOrientation *orientation_state); +ADDAPI void +ADDCALL psmove_orientation_get_quaternion(PSMoveOrientation *orientation_state, + float *q0, float *q1, float *q2, float *q3); + +ADDAPI void +ADDCALL psmove_orientation_reset_quaternion(PSMoveOrientation *orientation_state); #ifdef __cplusplus } diff --git a/src/tracker/CMakeLists.txt b/src/tracker/CMakeLists.txt index 9e15d42d..ec5ee22a 100644 --- a/src/tracker/CMakeLists.txt +++ b/src/tracker/CMakeLists.txt @@ -36,6 +36,14 @@ ENDIF() IF(PSMOVE_BUILD_TRACKER) FIND_PACKAGE(OpenCV QUIET) IF(OpenCV_FOUND) + IF ("${OpenCV_LIBS}" STREQUAL "") + message("OpenCV Libs was empty! Manually setting.") + list(APPEND OpenCV_LIBS opencv_core opencv_imgproc opencv_calib3d opencv_features2d opencv_highgui) + ENDIF() + IF ("${OpenCV_INCLUDE_DIRS}" STREQUAL "" AND ${OpenCV_DIR}) + message("OpenCV include dirs was empty! Manually setting.") + set (OpenCV_INCLUDE_DIRS ${OpenCV_DIR}\include) + ENDIF() list(APPEND PSMOVEAPI_TRACKER_REQUIRED_LIBS ${OpenCV_LIBS}) include_directories(${OpenCV_INCLUDE_DIR}) set(INFO_BUILD_TRACKER "Yes") @@ -197,7 +205,6 @@ endif() message("") message(" Tracker") message(" Tracker library: " ${INFO_BUILD_TRACKER}) -message(" AHRS algorithm: " ${INFO_AHRS_ALGORITHM}) feature_use_info("PS Eye support: " PSMOVE_USE_PSEYE) feature_use_info("HTML tracing: " PSMOVE_USE_TRACKER_TRACE) message(" Use CL Eye SDK: " ${INFO_USE_CL_EYE_SDK}) diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 6a199932..1bfd6e65 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -4,6 +4,7 @@ include(${ROOT_DIR}/cmake/common.cmake) # Essential utilities include_directories(${MSVC_INCLUDES}) +include_directories(${ROOT_DIR}/src) foreach(UTILITY moved psmovepair magnetometer_calibration psmove_dfu_mode psmove_auth_response psmove_get_firmware_info) add_executable(${UTILITY} ${CMAKE_CURRENT_LIST_DIR}/${UTILITY}.c ${MSVC_SRCS}) target_link_libraries(${UTILITY} psmoveapi_static ${MSVC_LIBS}) diff --git a/src/utils/magnetometer_calibration.c b/src/utils/magnetometer_calibration.c index 065e0d9f..bdac58ec 100644 --- a/src/utils/magnetometer_calibration.c +++ b/src/utils/magnetometer_calibration.c @@ -27,64 +27,224 @@ * POSSIBILITY OF SUCH DAMAGE. **/ +//-- includes ---- #include +#include +#include #include "psmove.h" +#include "psmove_orientation.h" +#include "math/psmove_vector.h" -#define LED_RANGE_TARGET 320 +//-- constants ----- +#define LED_RANGE_TARGET 320.f +#define STABILIZE_WAIT_TIME_MS 1000 +#define DESIRED_MAGNETOMETER_SAMPLE_COUNT 100 +enum eCalibrationState +{ + Calibration_MeasureBExtents, + Calibration_WaitForGravityAlignment, + Calibration_MeasureBDirection, + Calibration_Complete, +}; + +//-- prototypes ----- +static bool is_move_stable_and_aligned_with_gravity(PSMove *move); + +//-- public methods ---- int main(int arg, char** args) { + if (!psmove_init(PSMOVE_CURRENT_VERSION)) { + fprintf(stderr, "PS Move API init failed (wrong version?)\n"); + exit(1); + } + int i; int count = psmove_count_connected(); - for (i=0; i 100) { - percentage = 100; - } else if (percentage < 0) { - percentage = 0; - } - psmove_set_leds(move, (unsigned char)(2.5 * (100 - percentage)), (unsigned char)(2.5 * percentage), 0); - psmove_update_leds(move); - - if (pressed & Btn_MOVE) { - psmove_set_leds(move, 0, 0, 0); - psmove_update_leds(move); - psmove_save_magnetometer_calibration(move); - calibration_done = 1; - break; - } else if (range > old_range) { - old_range = range; - printf("\rPress the MOVE button when value stops changing: %d", range); - fflush(stdout); - } - } - - } - } else { - printf("Ignoring non-Bluetooth PS Move #%d\n", i); + + if (move == NULL) + { + printf("Failed to open PS Move #%d\n", i); + continue; } + + // Make sure accelerometer is calibrated + // Otherwise we can't tell if the controller is sitting upright + if (psmove_has_calibration(move)) + { + if (psmove_connection_type(move) == Conn_Bluetooth) + { + float old_range = 0; + enum eCalibrationState calibration_state = Calibration_MeasureBExtents; + + // Reset existing magnetometer calibration state + psmove_reset_magnetometer_calibration(move); + + printf("Calibrating PS Move #%d\n", i); + printf("[Step 1 of 2: Measuring extents of the magnetometer]\n"); + printf("Rotate the controller in all directions.\n"); + fflush(stdout); + + while (calibration_state == Calibration_MeasureBExtents) + { + if (psmove_poll(move)) + { + unsigned int pressed, released; + psmove_get_button_events(move, &pressed, &released); + + /* This call updates min/max values if exceeding previously stored values */ + psmove_get_magnetometer_3axisvector(move, NULL); + + /* Returns the minimum delta (max-min) across dimensions (x, y, z) */ + float range = psmove_get_magnetometer_calibration_range(move); + + /* Update the LEDs to indicate progress */ + int percentage = (int)(100.f * range / LED_RANGE_TARGET); + if (percentage > 100) + { + percentage = 100; + } + else if (percentage < 0) + { + percentage = 0; + } + + psmove_set_leds( + move, + (unsigned char)((255 * (100 - percentage)) / 100), + (unsigned char)((255 * percentage) / 100), + 0); + psmove_update_leds(move); + + if (pressed & Btn_MOVE) + { + psmove_set_leds(move, 0, 0, 0); + psmove_update_leds(move); + + // Move on to the + calibration_state = Calibration_WaitForGravityAlignment; + } + else if (range > old_range) + { + old_range = range; + printf("\rPress the MOVE button when value stops changing: %.1f", range); + fflush(stdout); + } + } + } + + printf("\n\n[Step 2 of 2: Measuring default magnetic field direction]\n"); + printf("Stand the controller on a level surface with the Move button facing you.\n"); + printf("This will be the default orientation of the move controller.\n"); + printf("Measurement will start once the controller is aligned with gravity and stable.\n"); + fflush(stdout); + + //TODO: Wait for accelerometer stabilization and button press + // Sample the magnetometer field for several frames and average + // Store as the default magnetometer direction + while (calibration_state != Calibration_Complete) + { + int stable_start_time = psmove_util_get_ticks(); + PSMove_3AxisVector identity_pose_average_m_vector = *k_psmove_vector_zero; + int sample_count = 0; + + while (calibration_state == Calibration_WaitForGravityAlignment) + { + if (psmove_poll(move)) + { + if (is_move_stable_and_aligned_with_gravity(move)) + { + int current_time = psmove_util_get_ticks(); + int stable_duration = (current_time - stable_start_time); + + if ((current_time - stable_start_time) >= STABILIZE_WAIT_TIME_MS) + { + calibration_state = Calibration_MeasureBDirection; + } + else + { + printf("\rStable for: %dms/%dms ", stable_duration, STABILIZE_WAIT_TIME_MS); + } + } + else + { + stable_start_time = psmove_util_get_ticks(); + printf("\rMove Destabilized! Waiting for stabilization."); + } + } + } + + printf("\n\nMove stabilized. Starting magnetometer sampling.\n"); + while (calibration_state == Calibration_MeasureBDirection) + { + if (psmove_poll(move)) + { + if (is_move_stable_and_aligned_with_gravity(move)) + { + PSMove_3AxisVector m; + + psmove_get_magnetometer_3axisvector(move, &m); + psmove_3axisvector_normalize_with_default(&m, k_psmove_vector_zero); + + identity_pose_average_m_vector = psmove_3axisvector_add(&identity_pose_average_m_vector, &m); + sample_count++; + + if (sample_count > DESIRED_MAGNETOMETER_SAMPLE_COUNT) + { + float N = (float)sample_count; + + // The average magnetometer direction was recorded while the controller + // was in the cradle pose + identity_pose_average_m_vector = psmove_3axisvector_divide_by_scalar_unsafe(&identity_pose_average_m_vector, N); + + psmove_set_magnetometer_calibration_direction(move, &identity_pose_average_m_vector); + + calibration_state = Calibration_Complete; + } + else + { + printf("\rMagnetometer Sample: %d/%d ", sample_count, DESIRED_MAGNETOMETER_SAMPLE_COUNT); + } + } + else + { + calibration_state = Calibration_WaitForGravityAlignment; + printf("\rMove Destabilized! Waiting for stabilization.\n"); + } + } + } + } + + psmove_save_magnetometer_calibration(move); + } + else + { + printf("Ignoring non-Bluetooth PS Move #%d\n", i); + } + } + else + { + char *serial= psmove_get_serial(move); + + printf("\nController #%d has bad calibration file (accelerometer values won't be correct).\n", i); + + if (serial != NULL) + { + printf("Please delete %s.calibration and re-run psmovepair.exe with the controller plugged into usb.", serial); + free(serial); + } + else + { + printf("Please re-run psmovepair.exe with the controller plugged into usb."); + } + } + printf("\nFinished PS Move #%d\n", i); psmove_disconnect(move); } @@ -92,3 +252,27 @@ main(int arg, char** args) return 0; } +//-- private methods ----- +static bool +is_move_stable_and_aligned_with_gravity(PSMove *move) +{ + const float k_cosine_10_degrees = 0.984808f; + + PSMove_3AxisVector k_identity_gravity_vector; + PSMove_3AxisVector acceleration_direction; + float acceleration_magnitude; + bool isOk; + + // Get the direction the gravity vector should be pointing + // while the controller is in cradle pose. + psmove_get_identity_gravity_calibration_direction(move, &k_identity_gravity_vector); + psmove_get_accelerometer_frame(move, Frame_SecondHalf, + &acceleration_direction.x, &acceleration_direction.y, &acceleration_direction.z); + acceleration_magnitude = psmove_3axisvector_normalize_with_default(&acceleration_direction, k_psmove_vector_zero); + + isOk = + is_nearly_equal(1.f, acceleration_magnitude, 0.1f) && + psmove_3axisvector_dot(&k_identity_gravity_vector, &acceleration_direction) >= k_cosine_10_degrees; + + return isOk; +} \ No newline at end of file