http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.cpp b/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.cpp new file mode 100644 index 0000000..78c099f --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.cpp @@ -0,0 +1,238 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#include "RTFusionKalman4.h" +#include "RTIMUSettings.h" + +// The QVALUE affects the gyro response. + +#define KALMAN_QVALUE 0.001f + +// The RVALUE controls the influence of the accels and compass. +// The bigger the value, the more sluggish the response. + +#define KALMAN_RVALUE 0.0005f + +#define KALMAN_QUATERNION_LENGTH 4 + +#define KALMAN_STATE_LENGTH 4 // just the quaternion for the moment + + +RTFusionKalman4::RTFusionKalman4() +{ + reset(); +} + +RTFusionKalman4::~RTFusionKalman4() +{ +} + +void RTFusionKalman4::reset() +{ + m_firstTime = true; + m_fusionPose = RTVector3(); + m_fusionQPose.fromEuler(m_fusionPose); + m_gyro = RTVector3(); + m_accel = RTVector3(); + m_compass = RTVector3(); + m_measuredPose = RTVector3(); + m_measuredQPose.fromEuler(m_measuredPose); + m_Rk.fill(0); + m_Q.fill(0); + + // initialize process noise covariance matrix + + for (int i = 0; i < KALMAN_STATE_LENGTH; i++) + for (int j = 0; j < KALMAN_STATE_LENGTH; j++) + m_Q.setVal(i, i, KALMAN_QVALUE); + + // initialize observation noise covariance matrix + + + for (int i = 0; i < KALMAN_STATE_LENGTH; i++) + for (int j = 0; j < KALMAN_STATE_LENGTH; j++) + m_Rk.setVal(i, i, KALMAN_RVALUE); + } + +void RTFusionKalman4::predict() +{ + RTMatrix4x4 mat; + RTQuaternion tQuat; + RTFLOAT x2, y2, z2; + + // compute the state transition matrix + + x2 = m_gyro.x() / (RTFLOAT)2.0; + y2 = m_gyro.y() / (RTFLOAT)2.0; + z2 = m_gyro.z() / (RTFLOAT)2.0; + + m_Fk.setVal(0, 1, -x2); + m_Fk.setVal(0, 2, -y2); + m_Fk.setVal(0, 3, -z2); + + m_Fk.setVal(1, 0, x2); + m_Fk.setVal(1, 2, z2); + m_Fk.setVal(1, 3, -y2); + + m_Fk.setVal(2, 0, y2); + m_Fk.setVal(2, 1, -z2); + m_Fk.setVal(2, 3, x2); + + m_Fk.setVal(3, 0, z2); + m_Fk.setVal(3, 1, y2); + m_Fk.setVal(3, 2, -x2); + + m_FkTranspose = m_Fk.transposed(); + + // Predict new state estimate Xkk_1 = Fk * Xk_1k_1 + + tQuat = m_Fk * m_stateQ; + tQuat *= m_timeDelta; + m_stateQ += tQuat; + +// m_stateQ.normalize(); + + // Compute PDot = Fk * Pk_1k_1 + Pk_1k_1 * FkTranspose (note Pkk == Pk_1k_1 at this stage) + + m_PDot = m_Fk * m_Pkk; + mat = m_Pkk * m_FkTranspose; + m_PDot += mat; + + // add in Q to get the new prediction + + m_Pkk_1 = m_PDot + m_Q; + + // multiply by deltaTime (variable name is now misleading though) + + m_Pkk_1 *= m_timeDelta; +} + + +void RTFusionKalman4::update() +{ + RTQuaternion delta; + RTMatrix4x4 Sk, SkInverse; + + if (m_enableCompass || m_enableAccel) { + m_stateQError = m_measuredQPose - m_stateQ; + } else { + m_stateQError = RTQuaternion(); + } + + // Compute residual covariance Sk = Hk * Pkk_1 * HkTranspose + Rk + // Note: since Hk is the identity matrix, this has been simplified + + Sk = m_Pkk_1 + m_Rk; + + // Compute Kalman gain Kk = Pkk_1 * HkTranspose * SkInverse + // Note: again, the HkTranspose part is omitted + + SkInverse = Sk.inverted(); + + m_Kk = m_Pkk_1 * SkInverse; + + if (m_debug) + HAL_INFO(RTMath::display("Gain", m_Kk)); + + // make new state estimate + + delta = m_Kk * m_stateQError; + + m_stateQ += delta; + + m_stateQ.normalize(); + + // produce new estimate covariance Pkk = (I - Kk * Hk) * Pkk_1 + // Note: since Hk is the identity matrix, it is omitted + + m_Pkk.setToIdentity(); + m_Pkk -= m_Kk; + m_Pkk = m_Pkk * m_Pkk_1; + + if (m_debug) + HAL_INFO(RTMath::display("Cov", m_Pkk)); +} + +void RTFusionKalman4::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings) +{ + if (m_enableGyro) + m_gyro = data.gyro; + else + m_gyro = RTVector3(); + m_accel = data.accel; + m_compass = data.compass; + m_compassValid = data.compassValid; + + if (m_firstTime) { + m_lastFusionTime = data.timestamp; + calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination); + m_Fk.fill(0); + + // init covariance matrix to something + + m_Pkk.fill(0); + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + m_Pkk.setVal(i,j, 0.5); + + // initialize the observation model Hk + // Note: since the model is the state vector, this is an identity matrix so it won't be used + + // initialize the poses + + m_stateQ.fromEuler(m_measuredPose); + m_fusionQPose = m_stateQ; + m_fusionPose = m_measuredPose; + m_firstTime = false; + } else { + m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000; + m_lastFusionTime = data.timestamp; + if (m_timeDelta <= 0) + return; + + if (m_debug) { + HAL_INFO("\n------\n"); + HAL_INFO1("IMU update delta time: %f\n", m_timeDelta); + } + + calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination); + + predict(); + update(); + m_stateQ.toEuler(m_fusionPose); + m_fusionQPose = m_stateQ; + + if (m_debug) { + HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose)); + HAL_INFO(RTMath::displayRadians("Kalman pose", m_fusionPose)); + HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose)); + HAL_INFO(RTMath::display("Kalman quat", m_stateQ)); + HAL_INFO(RTMath::display("Error quat", m_stateQError)); + } + } + data.fusionPoseValid = true; + data.fusionQPoseValid = true; + data.fusionPose = m_fusionPose; + data.fusionQPose = m_fusionQPose; +}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.h b/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.h new file mode 100644 index 0000000..245ae6f --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.h @@ -0,0 +1,79 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#ifndef _RTFUSIONKALMAN4_H +#define _RTFUSIONKALMAN4_H + +#include "RTFusion.h" + +class RTFusionKalman4 : public RTFusion +{ +public: + RTFusionKalman4(); + ~RTFusionKalman4(); + + // fusionType returns the type code of the fusion algorithm + + virtual int fusionType() { return RTFUSION_TYPE_KALMANSTATE4; } + + // reset() resets the kalman state but keeps any setting changes (such as enables) + + void reset(); + + // newIMUData() should be called for subsequent updates + // deltaTime is in units of seconds + + void newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings); + + // the following two functions can be called to customize the covariance matrices + + void setQMatrix(RTMatrix4x4 Q) { m_Q = Q; reset();} + void setRkMatrix(RTMatrix4x4 Rk) { m_Rk = Rk; reset();} + +private: + void predict(); + void update(); + + RTVector3 m_gyro; // unbiased gyro data + RTFLOAT m_timeDelta; // time between predictions + + RTQuaternion m_stateQ; // quaternion state vector + RTQuaternion m_stateQError; // difference between stateQ and measuredQ + + RTMatrix4x4 m_Kk; // the Kalman gain matrix + RTMatrix4x4 m_Pkk_1; // the predicted estimated covariance matrix + RTMatrix4x4 m_Pkk; // the updated estimated covariance matrix + RTMatrix4x4 m_PDot; // the derivative of the covariance matrix + RTMatrix4x4 m_Q; // process noise covariance + RTMatrix4x4 m_Fk; // the state transition matrix + RTMatrix4x4 m_FkTranspose; // the state transition matrix transposed + RTMatrix4x4 m_Rk; // the measurement noise covariance + + // Note: SInce Hk ends up being the identity matrix, these are omitted + +// RTMatrix4x4 m_Hk; // map from state to measurement +// RTMatrix4x4> m_HkTranspose; // transpose of map +}; + +#endif // _RTFUSIONKALMAN4_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.cpp b/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.cpp new file mode 100644 index 0000000..3842d6e --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.cpp @@ -0,0 +1,163 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#include "RTFusionRTQF.h" +#include "RTIMUSettings.h" + + +RTFusionRTQF::RTFusionRTQF() +{ + reset(); +} + +RTFusionRTQF::~RTFusionRTQF() +{ +} + +void RTFusionRTQF::reset() +{ + m_firstTime = true; + m_fusionPose = RTVector3(); + m_fusionQPose.fromEuler(m_fusionPose); + m_gyro = RTVector3(); + m_accel = RTVector3(); + m_compass = RTVector3(); + m_measuredPose = RTVector3(); + m_measuredQPose.fromEuler(m_measuredPose); + m_sampleNumber = 0; + } + +void RTFusionRTQF::predict() +{ + RTFLOAT x2, y2, z2; + RTFLOAT qs, qx, qy,qz; + + if (!m_enableGyro) + return; + + qs = m_stateQ.scalar(); + qx = m_stateQ.x(); + qy = m_stateQ.y(); + qz = m_stateQ.z(); + + x2 = m_gyro.x() / (RTFLOAT)2.0; + y2 = m_gyro.y() / (RTFLOAT)2.0; + z2 = m_gyro.z() / (RTFLOAT)2.0; + + // Predict new state + + m_stateQ.setScalar(qs + (-x2 * qx - y2 * qy - z2 * qz) * m_timeDelta); + m_stateQ.setX(qx + (x2 * qs + z2 * qy - y2 * qz) * m_timeDelta); + m_stateQ.setY(qy + (y2 * qs - z2 * qx + x2 * qz) * m_timeDelta); + m_stateQ.setZ(qz + (z2 * qs + y2 * qx - x2 * qy) * m_timeDelta); + m_stateQ.normalize(); +} + + +void RTFusionRTQF::update() +{ + if (m_enableCompass || m_enableAccel) { + + // calculate rotation delta + + m_rotationDelta = m_stateQ.conjugate() * m_measuredQPose; + m_rotationDelta.normalize(); + + // take it to the power (0 to 1) to give the desired amount of correction + + RTFLOAT theta = acos(m_rotationDelta.scalar()); + + RTFLOAT sinPowerTheta = sin(theta * m_slerpPower); + RTFLOAT cosPowerTheta = cos(theta * m_slerpPower); + + m_rotationUnitVector.setX(m_rotationDelta.x()); + m_rotationUnitVector.setY(m_rotationDelta.y()); + m_rotationUnitVector.setZ(m_rotationDelta.z()); + m_rotationUnitVector.normalize(); + + m_rotationPower.setScalar(cosPowerTheta); + m_rotationPower.setX(sinPowerTheta * m_rotationUnitVector.x()); + m_rotationPower.setY(sinPowerTheta * m_rotationUnitVector.y()); + m_rotationPower.setZ(sinPowerTheta * m_rotationUnitVector.z()); + m_rotationPower.normalize(); + + // multiple this by predicted value to get result + + m_stateQ *= m_rotationPower; + m_stateQ.normalize(); + } +} + +void RTFusionRTQF::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings) +{ + if (m_debug) { + HAL_INFO("\n------\n"); + HAL_INFO2("IMU update delta time: %f, sample %d\n", m_timeDelta, m_sampleNumber++); + } + m_sampleNumber++; + + if (m_enableGyro) + m_gyro = data.gyro; + else + m_gyro = RTVector3(); + m_accel = data.accel; + m_compass = data.compass; + m_compassValid = data.compassValid; + + if (m_firstTime) { + m_lastFusionTime = data.timestamp; + calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination); + + // initialize the poses + + m_stateQ.fromEuler(m_measuredPose); + m_fusionQPose = m_stateQ; + m_fusionPose = m_measuredPose; + m_firstTime = false; + } else { + m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000; + m_lastFusionTime = data.timestamp; + if (m_timeDelta <= 0) + return; + + calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination); + + predict(); + update(); + m_stateQ.toEuler(m_fusionPose); + m_fusionQPose = m_stateQ; + + if (m_debug) { + HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose)); + HAL_INFO(RTMath::displayRadians("RTQF pose", m_fusionPose)); + HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose)); + HAL_INFO(RTMath::display("RTQF quat", m_stateQ)); + HAL_INFO(RTMath::display("Error quat", m_stateQError)); + } + } + data.fusionPoseValid = true; + data.fusionQPoseValid = true; + data.fusionPose = m_fusionPose; + data.fusionQPose = m_fusionQPose; +} http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.h b/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.h new file mode 100644 index 0000000..854db34 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.h @@ -0,0 +1,62 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#ifndef _RTFUSIONRTQF_H +#define _RTFUSIONRTQF_H + +#include "RTFusion.h" + +class RTFusionRTQF : public RTFusion +{ +public: + RTFusionRTQF(); + ~RTFusionRTQF(); + + // fusionType returns the type code of the fusion algorithm + + virtual int fusionType() { return RTFUSION_TYPE_RTQF; } + + // reset() resets the state but keeps any setting changes (such as enables) + + void reset(); + + // newIMUData() should be called for subsequent updates + // deltaTime is in units of seconds + + void newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings); + +private: + void predict(); + void update(); + + RTVector3 m_gyro; // unbiased gyro data + RTFLOAT m_timeDelta; // time between predictions + + RTQuaternion m_stateQ; // quaternion state vector + RTQuaternion m_stateQError; // difference between stateQ and measuredQ + + int m_sampleNumber; +}; + +#endif // _RTFUSIONRTQF_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.cpp b/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.cpp new file mode 100644 index 0000000..20f4ec1 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.cpp @@ -0,0 +1,103 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include "RTIMUAccelCal.h" + +// ACCEL_ALPHA control the smoothing - the lower it is, the smoother it is + +#define ACCEL_ALPHA 0.1f + +RTIMUAccelCal::RTIMUAccelCal(RTIMUSettings *settings) +{ + m_settings = settings; + for (int i = 0; i < 3; i++) + m_accelCalEnable[i] = false; +} + +RTIMUAccelCal::~RTIMUAccelCal() +{ + +} + +void RTIMUAccelCal::accelCalInit() +{ + if (m_settings->m_accelCalValid) { + m_accelMin = m_settings->m_accelCalMin; + m_accelMax = m_settings->m_accelCalMax; + } else { + m_accelMin = RTVector3(RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN); + m_accelMax = RTVector3(RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX); + } +} + +void RTIMUAccelCal::accelCalReset() +{ + for (int i = 0; i < 3; i++) { + if (m_accelCalEnable[i]) { + m_accelMin.setData(i, RTIMUCALDEFS_DEFAULT_MIN); + m_accelMax.setData(i, RTIMUCALDEFS_DEFAULT_MAX); + } + } +} + +void RTIMUAccelCal::accelCalEnable(int axis, bool enable) +{ + m_accelCalEnable[axis] = enable; +} + +void RTIMUAccelCal::newAccelCalData(const RTVector3& data) +{ + + for (int i = 0; i < 3; i++) { + if (m_accelCalEnable[i]) { + m_averageValue.setData(i, (data.data(i) * ACCEL_ALPHA + m_averageValue.data(i) * (1.0 - ACCEL_ALPHA))); + if (m_accelMin.data(i) > m_averageValue.data(i)) + m_accelMin.setData(i, m_averageValue.data(i)); + if (m_accelMax.data(i) < m_averageValue.data(i)) + m_accelMax.setData(i, m_averageValue.data(i)); + } + } +} + +bool RTIMUAccelCal::accelCalValid() +{ + bool valid = true; + + for (int i = 0; i < 3; i++) { + if (m_accelMax.data(i) < m_accelMin.data(i)) + valid = false; + } + return valid; +} + +bool RTIMUAccelCal::accelCalSave() +{ + if (!accelCalValid()) + return false; + + m_settings->m_accelCalValid = true; + m_settings->m_accelCalMin = m_accelMin; + m_settings->m_accelCalMax = m_accelMax; + m_settings->saveSettings(); + return true; +} http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.h b/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.h new file mode 100644 index 0000000..1595712 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.h @@ -0,0 +1,74 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#ifndef _RTIMUACCELCAL_H +#define _RTIMUACCELCAL_H + +#include "RTIMUCalDefs.h" +#include "RTIMULib.h" + +// RTIMUAccelCal is a helper class for performing accelerometer calibration + +class RTIMUAccelCal +{ + +public: + RTIMUAccelCal(RTIMUSettings *settings); + virtual ~RTIMUAccelCal(); + + // This should be called at the start of the calibration process + // Loads previous values if available + void accelCalInit(); + + // This should be called to clear enabled axes for a new run + void accelCalReset(); + + // accelCalEnable() controls which axes are active - largely so that each can be done separately + void accelCalEnable(int axis, bool enable); + + // newAccalCalData() adds a new sample for processing but only the axes enabled previously + void newAccelCalData(const RTVector3& data); // adds a new accel sample + + // accelCalValid() checks if all values are reasonable. Should be called before saving + bool accelCalValid(); + + // accelCalSave() should be called at the end of the process to save the cal data + // to the settings file. Returns false if invalid data + bool accelCalSave(); // saves the accel cal data for specified axes + + // these vars used during the calibration process + + bool m_accelCalValid; // true if the mag min/max data valid + RTVector3 m_accelMin; // the min values + RTVector3 m_accelMax; // the max values + + RTVector3 m_averageValue; // averaged value actually used + + bool m_accelCalEnable[3]; // the enable flags + + RTIMUSettings *m_settings; + +}; + +#endif // _RTIMUACCELCAL_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUCalDefs.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUCalDefs.h b/thirdparty/RTIMULib/RTIMULib/RTIMUCalDefs.h new file mode 100644 index 0000000..54a7003 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUCalDefs.h @@ -0,0 +1,57 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#ifndef RTIMUCALDEFS_H +#define RTIMUCALDEFS_H + +#define RTIMUCALDEFS_DEFAULT_MIN 1000 // a large min +#define RTIMUCALDEFS_DEFAULT_MAX -1000 // a small max + +#define RTIMUCALDEFS_MAX_MAG_SAMPLES 20000 // max saved mag records + +#define RTIMUCALDEFS_OCTANT_MIN_SAMPLES 200 // must have at least this in each octant + +#define RTIMUCALDEFS_ELLIPSOID_MIN_SPACING 0.1f // min distnace between ellipsoid samples to be recorded + +// Octant defs + +#define RTIMUCALDEFS_OCTANT_COUNT 8 // there are 8 octants of course + +#define RTIMUCALDEFS_OCTANT_NNN 0 // x, y, z all negative +#define RTIMUCALDEFS_OCTANT_PNN 1 // x positive - y, z neagtive +#define RTIMUCALDEFS_OCTANT_NPN 2 // y positive - x, z negative +#define RTIMUCALDEFS_OCTANT_PPN 3 // x, y positive - z negative +#define RTIMUCALDEFS_OCTANT_NNP 4 // z positive - x, y negative +#define RTIMUCALDEFS_OCTANT_PNP 5 // x, z positive - y negative +#define RTIMUCALDEFS_OCTANT_NPP 6 // y, z positive - x negative +#define RTIMUCALDEFS_OCTANT_PPP 7 // x, y, z all positive + +// File name for Octave processing + +#define RTIMUCALDEFS_MAG_RAW_FILE "magRaw.dta" // the raw sample file - input to ellispoid fit code +#define RTIMUCALDEFS_MAG_CORR_FILE "magCorr.dta" // the output from the ellipsoid fit code + +#define RTIMUCALDEFS_OCTAVE_CODE "RTEllipsoidFit.m" +#define RTIMUCALDEFS_OCTAVE_COMMAND "octave RTEllipsoidFit.m" + +#endif // RTIMUCALDEFS_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUHal.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUHal.cpp b/thirdparty/RTIMULib/RTIMULib/RTIMUHal.cpp new file mode 100644 index 0000000..e70d916 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUHal.cpp @@ -0,0 +1,407 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +// The MPU-9250 and SPI driver code is based on code generously supplied by +// stasl...@gmail.com (www.clickdrive.io) + +#include "IMUDrivers/RTIMU.h" + +#if !defined(WIN32) && !defined(__APPLE__) + +#include <linux/spi/spidev.h> + +RTIMUHal::RTIMUHal() +{ + m_I2CBus = 255; + m_currentSlave = 255; + m_I2C = -1; + m_SPI = -1; + m_SPISpeed = 500000; +} + +RTIMUHal::~RTIMUHal() +{ + HALClose(); +} + +bool RTIMUHal::HALOpen() +{ + char buf[32]; + unsigned char SPIMode = SPI_MODE_0; + unsigned char SPIBits = 8; + uint32_t SPISpeed = m_SPISpeed; + + if (m_busIsI2C) { + if (m_I2C >= 0) + return true; + + if (m_I2CBus == 255) { + HAL_ERROR("No I2C bus has been set\n"); + return false; + } + sprintf(buf, "/dev/i2c-%d", m_I2CBus); + m_I2C = open(buf, O_RDWR); + if (m_I2C < 0) { + HAL_ERROR1("Failed to open I2C bus %d\n", m_I2CBus); + m_I2C = -1; + return false; + } + } else { + if (m_SPIBus == 255) { + HAL_ERROR("No SPI bus has been set\n"); + return false; + } + + sprintf(buf, "/dev/spidev%d.%d", m_SPIBus, m_SPISelect); + m_SPI = open(buf, O_RDWR); + if (m_SPI < 0) { + HAL_ERROR2("Failed to open SPI bus %d, select %d\n", m_SPIBus, m_SPISelect); + m_SPI = -1; + return false; + } + + if (ioctl(m_SPI, SPI_IOC_WR_MODE, &SPIMode) < 0) { + HAL_ERROR1("Failed to set WR SPI_MODE0 on bus %d", m_SPIBus); + close(m_SPIBus); + return false; + } + + if (ioctl(m_SPI, SPI_IOC_RD_MODE, &SPIMode) < 0) { + HAL_ERROR1("Failed to set RD SPI_MODE0 on bus %d", m_SPIBus); + close(m_SPIBus); + return false; + } + + if (ioctl(m_SPI, SPI_IOC_WR_BITS_PER_WORD, &SPIBits) < 0) { + HAL_ERROR1("Failed to set WR 8 bit mode on bus %d", m_SPIBus); + close(m_SPIBus); + return false; + } + + if (ioctl(m_SPI, SPI_IOC_RD_BITS_PER_WORD, &SPIBits) < 0) { + HAL_ERROR1("Failed to set RD 8 bit mode on bus %d", m_SPIBus); + close(m_SPIBus); + return false; + } + + if (ioctl(m_SPI, SPI_IOC_WR_MAX_SPEED_HZ, &SPISpeed) < 0) { + HAL_ERROR2("Failed to set WR %dHz on bus %d", SPISpeed, m_SPIBus); + close(m_SPIBus); + return false; + } + + if (ioctl(m_SPI, SPI_IOC_RD_MAX_SPEED_HZ, &SPISpeed) < 0) { + HAL_ERROR2("Failed to set RD %dHz on bus %d", SPISpeed, m_SPIBus); + close(m_SPIBus); + return false; + } + } + return true; +} + +void RTIMUHal::HALClose() +{ + I2CClose(); + SPIClose(); +} + +void RTIMUHal::I2CClose() +{ + if (m_I2C >= 0) { + close(m_I2C); + m_I2C = -1; + m_currentSlave = 255; + } +} + +void RTIMUHal::SPIClose() +{ + if (m_SPI >= 0) { + close(m_SPI); + m_SPI = -1; + } +} + +bool RTIMUHal::HALWrite(unsigned char slaveAddr, unsigned char regAddr, + unsigned char const data, const char *errorMsg) +{ + return HALWrite(slaveAddr, regAddr, 1, &data, errorMsg); +} + +bool RTIMUHal::HALWrite(unsigned char slaveAddr, unsigned char regAddr, + unsigned char length, unsigned char const *data, const char *errorMsg) +{ + int result; + unsigned char txBuff[MAX_WRITE_LEN + 1]; + char *ifType; + + if (m_busIsI2C) { + if (!I2CSelectSlave(slaveAddr, errorMsg)) + return false; + ifType = (char *)"I2C"; + } else { + ifType = (char *)"SPI"; + } + + if (length == 0) { + result = ifWrite(®Addr, 1); + + if (result < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR2("%s write of regAddr failed - %s\n", ifType, errorMsg); + return false; + } else if (result != 1) { + if (strlen(errorMsg) > 0) + HAL_ERROR2("%s write of regAddr failed (nothing written) - %s\n", ifType, errorMsg); + return false; + } + } else { + txBuff[0] = regAddr; + memcpy(txBuff + 1, data, length); + + result = ifWrite(txBuff, length + 1); + + if (result < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR3("%s data write of %d bytes failed - %s\n", ifType, length, errorMsg); + return false; + } else if (result < (int)length) { + if (strlen(errorMsg) > 0) + HAL_ERROR4("%s data write of %d bytes failed, only %d written - %s\n", ifType, length, result, errorMsg); + return false; + } + } + return true; +} + +bool RTIMUHal::ifWrite(unsigned char *data, unsigned char length) +{ + struct spi_ioc_transfer wrIOC; + + if (m_busIsI2C) { + return write(m_I2C, data, length); + } else { + memset(&wrIOC, 0, sizeof(wrIOC)); + wrIOC.tx_buf = (unsigned long) data; + wrIOC.rx_buf = 0; + wrIOC.len = length; + return ioctl(m_SPI, SPI_IOC_MESSAGE(1), &wrIOC); + } +} + + +bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char regAddr, unsigned char length, + unsigned char *data, const char *errorMsg) +{ + int tries, result, total; + unsigned char rxBuff[MAX_READ_LEN + 1]; + struct spi_ioc_transfer rdIOC; + + if (m_busIsI2C) { + if (!HALWrite(slaveAddr, regAddr, 0, NULL, errorMsg)) + return false; + + total = 0; + tries = 0; + + while ((total < length) && (tries < 5)) { + result = read(m_I2C, data + total, length - total); + + if (result < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR3("I2C read error from %d, %d - %s\n", slaveAddr, regAddr, errorMsg); + return false; + } + + total += result; + + if (total == length) + break; + + delayMs(10); + tries++; + } + + if (total < length) { + if (strlen(errorMsg) > 0) + HAL_ERROR3("I2C read from %d, %d failed - %s\n", slaveAddr, regAddr, errorMsg); + return false; + } + } else { + rxBuff[0] = regAddr | 0x80; + memcpy(rxBuff + 1, data, length); + memset(&rdIOC, 0, sizeof(rdIOC)); + rdIOC.tx_buf = (unsigned long) rxBuff; + rdIOC.rx_buf = (unsigned long) rxBuff; + rdIOC.len = length + 1; + + if (ioctl(m_SPI, SPI_IOC_MESSAGE(1), &rdIOC) < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR2("SPI read error from %d - %s\n", regAddr, errorMsg); + return false; + } + memcpy(data, rxBuff + 1, length); + } + return true; +} + +bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length, + unsigned char *data, const char *errorMsg) +{ + int tries, result, total; + unsigned char rxBuff[MAX_READ_LEN + 1]; + struct spi_ioc_transfer rdIOC; + + if (m_busIsI2C) { + if (!I2CSelectSlave(slaveAddr, errorMsg)) + return false; + + total = 0; + tries = 0; + + while ((total < length) && (tries < 5)) { + result = read(m_I2C, data + total, length - total); + + if (result < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR2("I2C read error from %d - %s\n", slaveAddr, errorMsg); + return false; + } + + total += result; + + if (total == length) + break; + + delayMs(10); + tries++; + } + + if (total < length) { + if (strlen(errorMsg) > 0) + HAL_ERROR2("I2C read from %d failed - %s\n", slaveAddr, errorMsg); + return false; + } + } else { + memset(&rdIOC, 0, sizeof(rdIOC)); + rdIOC.tx_buf = 0; + rdIOC.rx_buf = (unsigned long) rxBuff; + rdIOC.len = length; + + if (ioctl(m_SPI, SPI_IOC_MESSAGE(1), &rdIOC) < 0) { + if (strlen(errorMsg) > 0) + HAL_ERROR1("SPI read error from - %s\n", errorMsg); + return false; + } + memcpy(data, rxBuff, length); + } + return true; +} + + +bool RTIMUHal::I2CSelectSlave(unsigned char slaveAddr, const char *errorMsg) +{ + if (m_currentSlave == slaveAddr) + return true; + + if (!HALOpen()) { + HAL_ERROR1("Failed to open I2C port - %s\n", errorMsg); + return false; + } + + if (ioctl(m_I2C, I2C_SLAVE, slaveAddr) < 0) { + HAL_ERROR2("I2C slave select %d failed - %s\n", slaveAddr, errorMsg); + return false; + } + + m_currentSlave = slaveAddr; + + return true; +} + +void RTIMUHal::delayMs(int milliSeconds) +{ + usleep(1000 * milliSeconds); +} + +#else +// just dummy routines for Windows + +RTIMUHal::RTIMUHal() +{ +} + +RTIMUHal::~RTIMUHal() +{ +} + + +bool RTIMUHal::HALOpen() +{ + return true; +} + +void RTIMUHal::HALClose() +{ +} + +void RTIMUHal::I2CClose() +{ +} + +bool RTIMUHal::HALWrite(unsigned char , unsigned char , + unsigned char const , const char *) +{ + return true; +} + +bool RTIMUHal::HALWrite(unsigned char , unsigned char , + unsigned char , unsigned char const *, const char *) +{ + return true; +} + + +bool RTIMUHal::HALRead(unsigned char , unsigned char , unsigned char , + unsigned char *, const char *) +{ + return true; +} + +bool RTIMUHal::HALRead(unsigned char slaveAddr, unsigned char length, + unsigned char *data, const char *errorMsg) +{ + return true; +} + + +bool RTIMUHal::I2CSelectSlave(unsigned char , const char *) +{ + return true; +} + +void RTIMUHal::delayMs(int milliSeconds) +{ +} +#endif + http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUHal.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUHal.h b/thirdparty/RTIMULib/RTIMULib/RTIMUHal.h new file mode 100644 index 0000000..c3e4d75 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUHal.h @@ -0,0 +1,116 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +// The MPU-9250 and SPI driver code is based on code generously supplied by +// stasl...@gmail.com (www.clickdrive.io) + +#ifndef _RTIMUHAL_H +#define _RTIMUHAL_H + +#include <stdio.h> +#include <math.h> +#include <stdint.h> +#include <fcntl.h> +#include <string.h> +#include <stdlib.h> + +#ifndef HAL_QUIET +#define HAL_INFO(m) { printf("%s", m); fflush(stdout); } +#define HAL_INFO1(m, x) { printf(m, x); fflush(stdout); } +#define HAL_INFO2(m, x, y) { printf(m, x, y); fflush(stdout); } +#define HAL_INFO3(m, x, y, z) { printf(m, x, y, z); fflush(stdout); } +#define HAL_INFO4(m, x, y, z, a) { printf(m, x, y, z, a); fflush(stdout); } +#define HAL_INFO5(m, x, y, z, a, b) { printf(m, x, y, z, a, b); fflush(stdout); } +#define HAL_ERROR(m) fprintf(stderr, m); +#define HAL_ERROR1(m, x) fprintf(stderr, m, x); +#define HAL_ERROR2(m, x, y) fprintf(stderr, m, x, y); +#define HAL_ERROR3(m, x, y, z) fprintf(stderr, m, x, y, z); +#define HAL_ERROR4(m, x, y, z, a) fprintf(stderr, m, x, y, z, a); + +#else + +#define HAL_INFO(m) +#define HAL_INFO1(m, x) +#define HAL_INFO2(m, x, y) +#define HAL_INFO3(m, x, y, z) +#define HAL_INFO4(m, x, y, z, a) +#define HAL_INFO5(m, x, y, z, a, b) +#define HAL_ERROR(m) +#define HAL_ERROR1(m, x) +#define HAL_ERROR2(m, x, y) +#define HAL_ERROR3(m, x, y, z) +#define HAL_ERROR4(m, x, y, z, a) + +#endif + +#if !defined(WIN32) && !defined(__APPLE__) +#include <sys/ioctl.h> +#include <linux/i2c-dev.h> +#endif + +#if !defined(WIN32) +#include <unistd.h> +#include <sys/time.h> +#endif + +#define MAX_WRITE_LEN 255 +#define MAX_READ_LEN 255 + +class RTIMUHal +{ +public: + RTIMUHal(); + virtual ~RTIMUHal(); + + bool m_busIsI2C; // true if I2C bus in use, false if SPI in use + unsigned char m_I2CBus; // I2C bus of the imu (eg 1 for Raspberry Pi usually) + unsigned char m_SPIBus; // SPI bus of the imu (eg 0 for Raspberry Pi usually) + unsigned char m_SPISelect; // SPI select line - defaults to CE0 + unsigned int m_SPISpeed; // speed of interface + + bool HALOpen(); + void HALClose(); + bool HALRead(unsigned char slaveAddr, unsigned char regAddr, unsigned char length, + unsigned char *data, const char *errorMsg); // normal read with register select + bool HALRead(unsigned char slaveAddr, unsigned char length, + unsigned char *data, const char *errorMsg); // read without register select + bool HALWrite(unsigned char slaveAddr, unsigned char regAddr, + unsigned char length, unsigned char const *data, const char *errorMsg); + bool HALWrite(unsigned char slaveAddr, unsigned char regAddr, + unsigned char const data, const char *errorMsg); + + void delayMs(int milliSeconds); + +protected: + void I2CClose(); + bool I2CSelectSlave(unsigned char slaveAddr, const char *errorMsg); + void SPIClose(); + bool ifWrite(unsigned char *data, unsigned char length); + +private: + int m_I2C; + unsigned char m_currentSlave; + int m_SPI; +}; + +#endif // _RTIMUHAL_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMULIB LICENSE ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMULIB LICENSE b/thirdparty/RTIMULib/RTIMULib/RTIMULIB LICENSE new file mode 100644 index 0000000..cb4ed27 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMULIB LICENSE @@ -0,0 +1,26 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +// The MPU-9250 and SPI driver code is based on code generously supplied by +// stasl...@gmail.com (www.clickdrive.io) + http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMULib.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMULib.h b/thirdparty/RTIMULib/RTIMULib/RTIMULib.h new file mode 100644 index 0000000..cf851bb --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMULib.h @@ -0,0 +1,54 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#ifndef _RTIMULIB_H +#define _RTIMULIB_H + +#include "RTIMULibDefs.h" + +#include "RTMath.h" + +#include "RTFusion.h" +#include "RTFusionKalman4.h" + +#include "RTIMUHal.h" +#include "IMUDrivers/RTIMU.h" +#include "IMUDrivers/RTIMUNull.h" +#include "IMUDrivers/RTIMUMPU9150.h" +#include "IMUDrivers/RTIMUGD20HM303D.h" +#include "IMUDrivers/RTIMUGD20M303DLHC.h" +#include "IMUDrivers/RTIMULSM9DS0.h" + +#include "IMUDrivers/RTPressure.h" +#include "IMUDrivers/RTPressureBMP180.h" +#include "IMUDrivers/RTPressureLPS25H.h" +#include "IMUDrivers/RTPressureMS5611.h" + +#include "IMUDrivers/RTHumidity.h" +#include "IMUDrivers/RTHumidityHTS221.h" + +#include "RTIMUSettings.h" + + +#endif // _RTIMULIB_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMULib.pri ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMULib.pri b/thirdparty/RTIMULib/RTIMULib/RTIMULib.pri new file mode 100644 index 0000000..70b1da3 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMULib.pri @@ -0,0 +1,90 @@ +#//////////////////////////////////////////////////////////////////////////// +#// +#// This file is part of RTIMULib +#// +#// Copyright (c) 2014-2015, richards-tech, LLC +#// +#// Permission is hereby granted, free of charge, to any person obtaining a copy of +#// this software and associated documentation files (the "Software"), to deal in +#// the Software without restriction, including without limitation the rights to use, +#// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +#// Software, and to permit persons to whom the Software is furnished to do so, +#// subject to the following conditions: +#// +#// The above copyright notice and this permission notice shall be included in all +#// copies or substantial portions of the Software. +#// +#// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +#// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +#// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +#// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +#// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +#// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +INCLUDEPATH += $$PWD +DEPENDPATH += $$PWD + +HEADERS += $$PWD/RTIMULib.h \ + $$PWD/RTIMULibDefs.h \ + $$PWD/RTMath.h \ + $$PWD/RTIMUHal.h \ + $$PWD/RTFusion.h \ + $$PWD/RTFusionKalman4.h \ + $$PWD/RTFusionRTQF.h \ + $$PWD/RTIMUSettings.h \ + $$PWD/RTIMUMagCal.h \ + $$PWD/RTIMUAccelCal.h \ + $$PWD/RTIMUCalDefs.h \ + $$PWD/IMUDrivers/RTIMU.h \ + $$PWD/IMUDrivers/RTIMUDefs.h \ + $$PWD/IMUDrivers/RTIMUMPU9150.h \ + $$PWD/IMUDrivers/RTIMUMPU9250.h \ + $$PWD/IMUDrivers/RTIMUGD20HM303D.h \ + $$PWD/IMUDrivers/RTIMUGD20M303DLHC.h \ + $$PWD/IMUDrivers/RTIMUGD20HM303DLHC.h \ + $$PWD/IMUDrivers/RTIMULSM9DS0.h \ + $$PWD/IMUDrivers/RTIMULSM9DS1.h \ + $$PWD/IMUDrivers/RTIMUBMX055.h \ + $$PWD/IMUDrivers/RTIMUBNO055.h \ + $$PWD/IMUDrivers/RTIMUNull.h \ + $$PWD/IMUDrivers/RTPressure.h \ + $$PWD/IMUDrivers/RTPressureDefs.h \ + $$PWD/IMUDrivers/RTPressureBMP180.h \ + $$PWD/IMUDrivers/RTPressureLPS25H.h \ + $$PWD/IMUDrivers/RTPressureMS5611.h \ + $$PWD/IMUDrivers/RTPressureMS5637.h \ + $$PWD/IMUDrivers/RTHumidity.h \ + $$PWD/IMUDrivers/RTHumidityDefs.h \ + $$PWD/IMUDrivers/RTHumidityHTS221.h \ + $$PWD/IMUDrivers/RTHumidityHTU21D.h \ + +SOURCES += $$PWD/RTMath.cpp \ + $$PWD/RTIMUHal.cpp \ + $$PWD/RTFusion.cpp \ + $$PWD/RTFusionKalman4.cpp \ + $$PWD/RTFusionRTQF.cpp \ + $$PWD/RTIMUSettings.cpp \ + $$PWD/RTIMUMagCal.cpp \ + $$PWD/RTIMUAccelCal.cpp \ + $$PWD/IMUDrivers/RTIMU.cpp \ + $$PWD/IMUDrivers/RTIMUMPU9150.cpp \ + $$PWD/IMUDrivers/RTIMUMPU9250.cpp \ + $$PWD/IMUDrivers/RTIMUGD20HM303D.cpp \ + $$PWD/IMUDrivers/RTIMUGD20M303DLHC.cpp \ + $$PWD/IMUDrivers/RTIMUGD20HM303DLHC.cpp \ + $$PWD/IMUDrivers/RTIMULSM9DS0.cpp \ + $$PWD/IMUDrivers/RTIMULSM9DS1.cpp \ + $$PWD/IMUDrivers/RTIMUBMX055.cpp \ + $$PWD/IMUDrivers/RTIMUBNO055.cpp \ + $$PWD/IMUDrivers/RTIMUNull.cpp \ + $$PWD/IMUDrivers/RTPressure.cpp \ + $$PWD/IMUDrivers/RTPressureBMP180.cpp \ + $$PWD/IMUDrivers/RTPressureLPS25H.cpp \ + $$PWD/IMUDrivers/RTPressureMS5611.cpp \ + $$PWD/IMUDrivers/RTPressureMS5637.cpp \ + $$PWD/IMUDrivers/RTHumidity.cpp \ + $$PWD/IMUDrivers/RTHumidityHTS221.cpp \ + $$PWD/IMUDrivers/RTHumidityHTU21D.cpp \ + + + http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMULibDefs.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMULibDefs.h b/thirdparty/RTIMULib/RTIMULib/RTIMULibDefs.h new file mode 100644 index 0000000..0157258 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMULibDefs.h @@ -0,0 +1,64 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +// The MPU-9250 and SPI driver code is based on code generously supplied by +// stasl...@gmail.com (www.clickdrive.io) + +#ifndef _RTIMULIBDEFS_H +#define _RTIMULIBDEFS_H + +#include "RTMath.h" +#include "IMUDrivers/RTIMUDefs.h" + +// these defines describe the various fusion filter options + +#define RTFUSION_TYPE_NULL 0 // just a dummy to keep things happy if not needed +#define RTFUSION_TYPE_KALMANSTATE4 1 // kalman state is the quaternion pose +#define RTFUSION_TYPE_RTQF 2 // RT quaternion fusion + +#define RTFUSION_TYPE_COUNT 3 // number of fusion algorithm types + +// This is a convenience structure that can be used to pass IMU data around + +typedef struct +{ + uint64_t timestamp; + bool fusionPoseValid; + RTVector3 fusionPose; + bool fusionQPoseValid; + RTQuaternion fusionQPose; + bool gyroValid; + RTVector3 gyro; + bool accelValid; + RTVector3 accel; + bool compassValid; + RTVector3 compass; + bool pressureValid; + RTFLOAT pressure; + bool temperatureValid; + RTFLOAT temperature; + bool humidityValid; + RTFLOAT humidity; +} RTIMU_DATA; + +#endif // _RTIMULIBDEFS_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp b/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp new file mode 100644 index 0000000..d61e021 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp @@ -0,0 +1,260 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#include "RTIMUMagCal.h" + +RTIMUMagCal::RTIMUMagCal(RTIMUSettings *settings) +{ + m_settings = settings; +} + +RTIMUMagCal::~RTIMUMagCal() +{ + +} + +void RTIMUMagCal::magCalInit() +{ + magCalReset(); +} + +void RTIMUMagCal::magCalReset() +{ + m_magMin = RTVector3(RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN); + m_magMax = RTVector3(RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX); + + m_magCalCount = 0; + for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++) + m_octantCounts[i] = 0; + m_magCalInIndex = m_magCalOutIndex = 0; + + // throw away first few samples so we don't see any old calibrated samples + m_startCount = 100; +} + +void RTIMUMagCal::newMinMaxData(const RTVector3& data) +{ + if (m_startCount > 0) { + m_startCount--; + return; + } + + for (int i = 0; i < 3; i++) { + if (m_magMin.data(i) > data.data(i)) { + m_magMin.setData(i, data.data(i)); + } + + if (m_magMax.data(i) < data.data(i)) { + m_magMax.setData(i, data.data(i)); + } + } +} + +bool RTIMUMagCal::magCalValid() +{ + bool valid = true; + + for (int i = 0; i < 3; i++) { + if (m_magMax.data(i) < m_magMin.data(i)) + valid = false; + } + return valid; + +} + +void RTIMUMagCal::magCalSaveMinMax() +{ + m_settings->m_compassCalValid = true; + m_settings->m_compassCalMin = m_magMin; + m_settings->m_compassCalMax = m_magMax; + m_settings->m_compassCalEllipsoidValid = false; + m_settings->saveSettings(); + + // need to invalidate ellipsoid data in order to use new min/max data + + m_magCalCount = 0; + for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++) + m_octantCounts[i] = 0; + m_magCalInIndex = m_magCalOutIndex = 0; + + // and set up for min/max calibration + + setMinMaxCal(); +} + +void RTIMUMagCal::newEllipsoidData(const RTVector3& data) +{ + RTVector3 calData; + + // do min/max calibration first + + for (int i = 0; i < 3; i++) + calData.setData(i, (data.data(i) - m_minMaxOffset.data(i)) * m_minMaxScale.data(i)); + + // now see if it's already there - we want them all unique and slightly separate (using a fuzzy compare) + + for (int index = m_magCalOutIndex, i = 0; i < m_magCalCount; i++) { + if ((abs(calData.x() - m_magCalSamples[index].x()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING) && + (abs(calData.y() - m_magCalSamples[index].y()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING) && + (abs(calData.z() - m_magCalSamples[index].z()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING)) { + return; // too close to another sample + } + if (++index == RTIMUCALDEFS_MAX_MAG_SAMPLES) + index = 0; + } + + + m_octantCounts[findOctant(calData)]++; + + m_magCalSamples[m_magCalInIndex++] = calData; + if (m_magCalInIndex == RTIMUCALDEFS_MAX_MAG_SAMPLES) + m_magCalInIndex = 0; + + if (++m_magCalCount == RTIMUCALDEFS_MAX_MAG_SAMPLES) { + // buffer is full - pull oldest + removeMagCalData(); + } +} + +bool RTIMUMagCal::magCalEllipsoidValid() +{ + bool valid = true; + + for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++) { + if (m_octantCounts[i] < RTIMUCALDEFS_OCTANT_MIN_SAMPLES) + valid = false; + } + return valid; +} + +RTVector3 RTIMUMagCal::removeMagCalData() +{ + RTVector3 ret; + + if (m_magCalCount == 0) + return ret; + + ret = m_magCalSamples[m_magCalOutIndex++]; + if (m_magCalOutIndex == RTIMUCALDEFS_MAX_MAG_SAMPLES) + m_magCalOutIndex = 0; + m_magCalCount--; + m_octantCounts[findOctant(ret)]--; + return ret; +} + +bool RTIMUMagCal::magCalSaveRaw(const char *ellipsoidFitPath) +{ + FILE *file; + char *rawFile; + + if (ellipsoidFitPath != NULL) { + // need to deal with ellipsoid fit processing + rawFile = (char *)malloc(strlen(RTIMUCALDEFS_MAG_RAW_FILE) + strlen(ellipsoidFitPath) + 2); + sprintf(rawFile, "%s/%s", ellipsoidFitPath, RTIMUCALDEFS_MAG_RAW_FILE); + if ((file = fopen(rawFile, "w")) == NULL) { + HAL_ERROR("Failed to open ellipsoid fit raw data file\n"); + return false; + } + while (m_magCalCount > 0) { + RTVector3 sample = removeMagCalData(); + fprintf(file, "%f %f %f\n", sample.x(), sample.y(), sample.z()); + } + fclose(file); + } + return true; +} + +bool RTIMUMagCal::magCalSaveCorr(const char *ellipsoidFitPath) +{ + FILE *file; + char *corrFile; + float a[3]; + float b[9]; + + if (ellipsoidFitPath != NULL) { + corrFile = (char *)malloc(strlen(RTIMUCALDEFS_MAG_CORR_FILE) + strlen(ellipsoidFitPath) + 2); + sprintf(corrFile, "%s/%s", ellipsoidFitPath, RTIMUCALDEFS_MAG_CORR_FILE); + if ((file = fopen(corrFile, "r")) == NULL) { + HAL_ERROR("Failed to open ellipsoid fit correction data file\n"); + return false; + } + if (fscanf(file, "%f %f %f %f %f %f %f %f %f %f %f %f", + a + 0, a + 1, a + 2, b + 0, b + 1, b + 2, b + 3, b + 4, b + 5, b + 6, b + 7, b + 8) != 12) { + HAL_ERROR("Ellipsoid corrcetion file didn't have 12 floats\n"); + fclose(file); + return false; + } + fclose(file); + m_settings->m_compassCalEllipsoidValid = true; + m_settings->m_compassCalEllipsoidOffset = RTVector3(a[0], a[1], a[2]); + memcpy(m_settings->m_compassCalEllipsoidCorr, b, 9 * sizeof(float)); + m_settings->saveSettings(); + return true; + } + return false; +} + + +void RTIMUMagCal::magCalOctantCounts(int *counts) +{ + memcpy(counts, m_octantCounts, RTIMUCALDEFS_OCTANT_COUNT * sizeof(int)); +} + +int RTIMUMagCal::findOctant(const RTVector3& data) +{ + int val = 0; + + if (data.x() >= 0) + val = 1; + if (data.y() >= 0) + val |= 2; + if (data.z() >= 0) + val |= 4; + + return val; +} + +void RTIMUMagCal::setMinMaxCal() +{ + float maxDelta = -1; + float delta; + + // find biggest range + + for (int i = 0; i < 3; i++) { + if ((m_magMax.data(i) - m_magMin.data(i)) > maxDelta) + maxDelta = m_magMax.data(i) - m_magMin.data(i); + } + if (maxDelta < 0) { + HAL_ERROR("Error in min/max calibration data\n"); + return; + } + maxDelta /= 2.0f; // this is the max +/- range + + for (int i = 0; i < 3; i++) { + delta = (m_magMax.data(i) -m_magMin.data(i)) / 2.0f; + m_minMaxScale.setData(i, maxDelta / delta); // makes everything the same range + m_minMaxOffset.setData(i, (m_magMax.data(i) + m_magMin.data(i)) / 2.0f); + } +} http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.h b/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.h new file mode 100644 index 0000000..463fdf4 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.h @@ -0,0 +1,98 @@ +//////////////////////////////////////////////////////////////////////////// +// +// This file is part of RTIMULib +// +// Copyright (c) 2014-2015, richards-tech, LLC +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of +// this software and associated documentation files (the "Software"), to deal in +// the Software without restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the +// Software, and to permit persons to whom the Software is furnished to do so, +// subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#ifndef _RTIMUMAGCAL_H +#define _RTIMUMAGCAL_H + +#include "RTIMUCalDefs.h" +#include "RTIMULib.h" + +class RTIMUMagCal +{ + +public: + RTIMUMagCal(RTIMUSettings *settings); + virtual ~RTIMUMagCal(); + + void magCalInit(); // inits everything + void magCalReset(); // clears everything + + // newMinMaxData() is used to submit a new sample for min/max processing + void newMinMaxData(const RTVector3& data); + + // newEllipsoidData is used to save data to the ellipsoid sample array + void newEllipsoidData(const RTVector3& data); + + // magCalValid() determines if the min/max data is basically valid + bool magCalValid(); + + // magCalEllipsoidValid() determines if enough samples have been collected for a valid ellipsoid fit + bool magCalEllipsoidValid(); + + // magCalSaveMinMax() saves the current min/max values to settings + void magCalSaveMinMax(); + + // magCalSaveRaw saves the ellipsoid fit data and then + // saves data to the .ini file. + // + // Returns true if everything worked correctly. + + bool magCalSaveRaw(const char *ellipsoidFitPath); + + // magCalSaveCorr loads the correction data from the ellipsoid fit program and saves it in the + // .ini + + bool magCalSaveCorr(const char *ellipsoidFitPath); + + // magCalSaveEllipsoid retrieves the ellipsoid fit calibration data + // and saves it in the .ini file. + + void magCalOctantCounts(int *counts); // returns a count for each of the 8 octants + + // these vars used during the calibration process + + RTVector3 m_magMin; // the min values + RTVector3 m_magMax; // the max values + + RTIMUSettings *m_settings; + +private: + RTVector3 removeMagCalData(); // takes an entry out of the buffer + int findOctant(const RTVector3& data); // works out which octant the data is in + void setMinMaxCal(); // get ready for the ellipsoid mode + + int m_startCount; // need to throw way first few samples + RTVector3 m_magCalSamples[RTIMUCALDEFS_MAX_MAG_SAMPLES];// the saved samples for ellipsoid fit + int m_magCalInIndex; // current in index into the data + int m_magCalOutIndex; // current out index into the data + int m_magCalCount; // how many samples in the buffer + + RTVector3 m_minMaxOffset; // the min/max calibration offset + RTVector3 m_minMaxScale; // the min/max scale + + int m_octantCounts[RTIMUCALDEFS_OCTANT_COUNT]; // counts in each octant + +}; + +#endif // _RTIMUMAGCAL_H
