You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nifi.apache.org by al...@apache.org on 2018/06/06 15:58:31 UTC
[1/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Repository: nifi-minifi-cpp
Updated Branches:
refs/heads/master 3156c1e00 -> 9dbad3bde
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTMath.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTMath.cpp b/thirdparty/RTIMULib/RTIMULib/RTMath.cpp
new file mode 100644
index 0000000..686817d
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTMath.cpp
@@ -0,0 +1,630 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTMath.h"
+#ifdef WIN32
+#include <qdatetime.h>
+#endif
+
+// Strings are put here. So the display functions are no re-entrant!
+
+char RTMath::m_string[1000];
+
+uint64_t RTMath::currentUSecsSinceEpoch()
+{
+#ifdef WIN32
+#include <qdatetime.h>
+ return QDateTime::currentMSecsSinceEpoch();
+#else
+ struct timeval tv;
+
+ gettimeofday(&tv, NULL);
+ return (uint64_t)tv.tv_sec * 1000000 + (uint64_t)tv.tv_usec;
+#endif
+}
+
+const char *RTMath::displayRadians(const char *label, RTVector3& vec)
+{
+ sprintf(m_string, "%s: x:%f, y:%f, z:%f\n", label, vec.x(), vec.y(), vec.z());
+ return m_string;
+}
+
+const char *RTMath::displayDegrees(const char *label, RTVector3& vec)
+{
+ sprintf(m_string, "%s: roll:%f, pitch:%f, yaw:%f", label, vec.x() * RTMATH_RAD_TO_DEGREE,
+ vec.y() * RTMATH_RAD_TO_DEGREE, vec.z() * RTMATH_RAD_TO_DEGREE);
+ return m_string;
+}
+
+const char *RTMath::display(const char *label, RTQuaternion& quat)
+{
+ sprintf(m_string, "%s: scalar: %f, x:%f, y:%f, z:%f\n", label, quat.scalar(), quat.x(), quat.y(), quat.z());
+ return m_string;
+}
+
+const char *RTMath::display(const char *label, RTMatrix4x4& mat)
+{
+ sprintf(m_string, "%s(0): %f %f %f %f\n%s(1): %f %f %f %f\n%s(2): %f %f %f %f\n%s(3): %f %f %f %f\n",
+ label, mat.val(0,0), mat.val(0,1), mat.val(0,2), mat.val(0,3),
+ label, mat.val(1,0), mat.val(1,1), mat.val(1,2), mat.val(1,3),
+ label, mat.val(2,0), mat.val(2,1), mat.val(2,2), mat.val(2,3),
+ label, mat.val(3,0), mat.val(3,1), mat.val(3,2), mat.val(3,3));
+ return m_string;
+}
+
+// convertPressureToHeight() - the conversion uses the formula:
+//
+// h = (T0 / L0) * ((p / P0)**(-(R* * L0) / (g0 * M)) - 1)
+//
+// where:
+// h = height above sea level
+// T0 = standard temperature at sea level = 288.15
+// L0 = standard temperatur elapse rate = -0.0065
+// p = measured pressure
+// P0 = static pressure = 1013.25 (but can be overridden)
+// g0 = gravitational acceleration = 9.80665
+// M = mloecular mass of earth's air = 0.0289644
+// R* = universal gas constant = 8.31432
+//
+// Given the constants, this works out to:
+//
+// h = 44330.8 * (1 - (p / P0)**0.190263)
+
+RTFLOAT RTMath::convertPressureToHeight(RTFLOAT pressure, RTFLOAT staticPressure)
+{
+ return 44330.8 * (1 - pow(pressure / staticPressure, (RTFLOAT)0.190263));
+}
+
+
+RTVector3 RTMath::poseFromAccelMag(const RTVector3& accel, const RTVector3& mag)
+{
+ RTVector3 result;
+ RTQuaternion m;
+ RTQuaternion q;
+
+ accel.accelToEuler(result);
+
+// q.fromEuler(result);
+// since result.z() is always 0, this can be optimized a little
+
+ RTFLOAT cosX2 = cos(result.x() / 2.0f);
+ RTFLOAT sinX2 = sin(result.x() / 2.0f);
+ RTFLOAT cosY2 = cos(result.y() / 2.0f);
+ RTFLOAT sinY2 = sin(result.y() / 2.0f);
+
+ q.setScalar(cosX2 * cosY2);
+ q.setX(sinX2 * cosY2);
+ q.setY(cosX2 * sinY2);
+ q.setZ(-sinX2 * sinY2);
+// q.normalize();
+
+ m.setScalar(0);
+ m.setX(mag.x());
+ m.setY(mag.y());
+ m.setZ(mag.z());
+
+ m = q * m * q.conjugate();
+ result.setZ(-atan2(m.y(), m.x()));
+ return result;
+}
+
+void RTMath::convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian)
+{
+ if (bigEndian) {
+ vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[0] << 8) | (uint16_t)rawData[1])) * scale);
+ vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[2] << 8) | (uint16_t)rawData[3])) * scale);
+ vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[4] << 8) | (uint16_t)rawData[5])) * scale);
+ } else {
+ vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[1] << 8) | (uint16_t)rawData[0])) * scale);
+ vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[3] << 8) | (uint16_t)rawData[2])) * scale);
+ vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[5] << 8) | (uint16_t)rawData[4])) * scale);
+ }
+}
+
+
+
+//----------------------------------------------------------
+//
+// The RTVector3 class
+
+RTVector3::RTVector3()
+{
+ zero();
+}
+
+RTVector3::RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z)
+{
+ m_data[0] = x;
+ m_data[1] = y;
+ m_data[2] = z;
+}
+
+RTVector3& RTVector3::operator =(const RTVector3& vec)
+{
+ if (this == &vec)
+ return *this;
+
+ m_data[0] = vec.m_data[0];
+ m_data[1] = vec.m_data[1];
+ m_data[2] = vec.m_data[2];
+
+ return *this;
+}
+
+
+const RTVector3& RTVector3::operator +=(RTVector3& vec)
+{
+ for (int i = 0; i < 3; i++)
+ m_data[i] += vec.m_data[i];
+ return *this;
+}
+
+const RTVector3& RTVector3::operator -=(RTVector3& vec)
+{
+ for (int i = 0; i < 3; i++)
+ m_data[i] -= vec.m_data[i];
+ return *this;
+}
+
+void RTVector3::zero()
+{
+ for (int i = 0; i < 3; i++)
+ m_data[i] = 0;
+}
+
+
+RTFLOAT RTVector3::dotProduct(const RTVector3& a, const RTVector3& b)
+{
+ return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
+}
+
+void RTVector3::crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d)
+{
+ d.setX(a.y() * b.z() - a.z() * b.y());
+ d.setY(a.z() * b.x() - a.x() * b.z());
+ d.setZ(a.x() * b.y() - a.y() * b.x());
+}
+
+
+void RTVector3::accelToEuler(RTVector3& rollPitchYaw) const
+{
+ RTVector3 normAccel = *this;
+
+ normAccel.normalize();
+
+ rollPitchYaw.setX(atan2(normAccel.y(), normAccel.z()));
+ rollPitchYaw.setY(-atan2(normAccel.x(), sqrt(normAccel.y() * normAccel.y() + normAccel.z() * normAccel.z())));
+ rollPitchYaw.setZ(0);
+}
+
+
+void RTVector3::accelToQuaternion(RTQuaternion& qPose) const
+{
+ RTVector3 normAccel = *this;
+ RTVector3 vec;
+ RTVector3 z(0, 0, 1.0);
+
+ normAccel.normalize();
+
+ RTFLOAT angle = acos(RTVector3::dotProduct(z, normAccel));
+ RTVector3::crossProduct(normAccel, z, vec);
+ vec.normalize();
+
+ qPose.fromAngleVector(angle, vec);
+}
+
+
+void RTVector3::normalize()
+{
+ RTFLOAT length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+ m_data[2] * m_data[2]);
+
+ if (length == 0)
+ return;
+
+ m_data[0] /= length;
+ m_data[1] /= length;
+ m_data[2] /= length;
+}
+
+RTFLOAT RTVector3::length()
+{
+ return sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+ m_data[2] * m_data[2]);
+}
+
+//----------------------------------------------------------
+//
+// The RTQuaternion class
+
+RTQuaternion::RTQuaternion()
+{
+ zero();
+}
+
+RTQuaternion::RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z)
+{
+ m_data[0] = scalar;
+ m_data[1] = x;
+ m_data[2] = y;
+ m_data[3] = z;
+}
+
+RTQuaternion& RTQuaternion::operator =(const RTQuaternion& quat)
+{
+ if (this == &quat)
+ return *this;
+
+ m_data[0] = quat.m_data[0];
+ m_data[1] = quat.m_data[1];
+ m_data[2] = quat.m_data[2];
+ m_data[3] = quat.m_data[3];
+
+ return *this;
+}
+
+
+
+RTQuaternion& RTQuaternion::operator +=(const RTQuaternion& quat)
+{
+ for (int i = 0; i < 4; i++)
+ m_data[i] += quat.m_data[i];
+ return *this;
+}
+
+RTQuaternion& RTQuaternion::operator -=(const RTQuaternion& quat)
+{
+ for (int i = 0; i < 4; i++)
+ m_data[i] -= quat.m_data[i];
+ return *this;
+}
+
+RTQuaternion& RTQuaternion::operator -=(const RTFLOAT val)
+{
+ for (int i = 0; i < 4; i++)
+ m_data[i] -= val;
+ return *this;
+}
+
+RTQuaternion& RTQuaternion::operator *=(const RTQuaternion& qb)
+{
+ RTQuaternion qa;
+
+ qa = *this;
+
+ m_data[0] = qa.scalar() * qb.scalar() - qa.x() * qb.x() - qa.y() * qb.y() - qa.z() * qb.z();
+ m_data[1] = qa.scalar() * qb.x() + qa.x() * qb.scalar() + qa.y() * qb.z() - qa.z() * qb.y();
+ m_data[2] = qa.scalar() * qb.y() - qa.x() * qb.z() + qa.y() * qb.scalar() + qa.z() * qb.x();
+ m_data[3] = qa.scalar() * qb.z() + qa.x() * qb.y() - qa.y() * qb.x() + qa.z() * qb.scalar();
+
+ return *this;
+}
+
+
+RTQuaternion& RTQuaternion::operator *=(const RTFLOAT val)
+{
+ m_data[0] *= val;
+ m_data[1] *= val;
+ m_data[2] *= val;
+ m_data[3] *= val;
+
+ return *this;
+}
+
+
+const RTQuaternion RTQuaternion::operator *(const RTQuaternion& qb) const
+{
+ RTQuaternion result = *this;
+ result *= qb;
+ return result;
+}
+
+const RTQuaternion RTQuaternion::operator *(const RTFLOAT val) const
+{
+ RTQuaternion result = *this;
+ result *= val;
+ return result;
+}
+
+
+const RTQuaternion RTQuaternion::operator -(const RTQuaternion& qb) const
+{
+ RTQuaternion result = *this;
+ result -= qb;
+ return result;
+}
+
+const RTQuaternion RTQuaternion::operator -(const RTFLOAT val) const
+{
+ RTQuaternion result = *this;
+ result -= val;
+ return result;
+}
+
+
+void RTQuaternion::zero()
+{
+ for (int i = 0; i < 4; i++)
+ m_data[i] = 0;
+}
+
+void RTQuaternion::normalize()
+{
+ RTFLOAT length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+ m_data[2] * m_data[2] + m_data[3] * m_data[3]);
+
+ if ((length == 0) || (length == 1))
+ return;
+
+ m_data[0] /= length;
+ m_data[1] /= length;
+ m_data[2] /= length;
+ m_data[3] /= length;
+}
+
+void RTQuaternion::toEuler(RTVector3& vec)
+{
+ vec.setX(atan2(2.0 * (m_data[2] * m_data[3] + m_data[0] * m_data[1]),
+ 1 - 2.0 * (m_data[1] * m_data[1] + m_data[2] * m_data[2])));
+
+ vec.setY(asin(2.0 * (m_data[0] * m_data[2] - m_data[1] * m_data[3])));
+
+ vec.setZ(atan2(2.0 * (m_data[1] * m_data[2] + m_data[0] * m_data[3]),
+ 1 - 2.0 * (m_data[2] * m_data[2] + m_data[3] * m_data[3])));
+}
+
+void RTQuaternion::fromEuler(RTVector3& vec)
+{
+ RTFLOAT cosX2 = cos(vec.x() / 2.0f);
+ RTFLOAT sinX2 = sin(vec.x() / 2.0f);
+ RTFLOAT cosY2 = cos(vec.y() / 2.0f);
+ RTFLOAT sinY2 = sin(vec.y() / 2.0f);
+ RTFLOAT cosZ2 = cos(vec.z() / 2.0f);
+ RTFLOAT sinZ2 = sin(vec.z() / 2.0f);
+
+ m_data[0] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
+ m_data[1] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
+ m_data[2] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
+ m_data[3] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
+ normalize();
+}
+
+RTQuaternion RTQuaternion::conjugate() const
+{
+ RTQuaternion q;
+ q.setScalar(m_data[0]);
+ q.setX(-m_data[1]);
+ q.setY(-m_data[2]);
+ q.setZ(-m_data[3]);
+ return q;
+}
+
+void RTQuaternion::toAngleVector(RTFLOAT& angle, RTVector3& vec)
+{
+ RTFLOAT halfTheta;
+ RTFLOAT sinHalfTheta;
+
+ halfTheta = acos(m_data[0]);
+ sinHalfTheta = sin(halfTheta);
+
+ if (sinHalfTheta == 0) {
+ vec.setX(1.0);
+ vec.setY(0);
+ vec.setZ(0);
+ } else {
+ vec.setX(m_data[1] / sinHalfTheta);
+ vec.setY(m_data[1] / sinHalfTheta);
+ vec.setZ(m_data[1] / sinHalfTheta);
+ }
+ angle = 2.0 * halfTheta;
+}
+
+void RTQuaternion::fromAngleVector(const RTFLOAT& angle, const RTVector3& vec)
+{
+ RTFLOAT sinHalfTheta = sin(angle / 2.0);
+ m_data[0] = cos(angle / 2.0);
+ m_data[1] = vec.x() * sinHalfTheta;
+ m_data[2] = vec.y() * sinHalfTheta;
+ m_data[3] = vec.z() * sinHalfTheta;
+}
+
+
+
+//----------------------------------------------------------
+//
+// The RTMatrix4x4 class
+
+RTMatrix4x4::RTMatrix4x4()
+{
+ fill(0);
+}
+
+RTMatrix4x4& RTMatrix4x4::operator =(const RTMatrix4x4& mat)
+{
+ if (this == &mat)
+ return *this;
+
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ m_data[row][col] = mat.m_data[row][col];
+
+ return *this;
+}
+
+
+void RTMatrix4x4::fill(RTFLOAT val)
+{
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ m_data[row][col] = val;
+}
+
+
+RTMatrix4x4& RTMatrix4x4::operator +=(const RTMatrix4x4& mat)
+{
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ m_data[row][col] += mat.m_data[row][col];
+
+ return *this;
+}
+
+RTMatrix4x4& RTMatrix4x4::operator -=(const RTMatrix4x4& mat)
+{
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ m_data[row][col] -= mat.m_data[row][col];
+
+ return *this;
+}
+
+RTMatrix4x4& RTMatrix4x4::operator *=(const RTFLOAT val)
+{
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ m_data[row][col] *= val;
+
+ return *this;
+}
+
+const RTMatrix4x4 RTMatrix4x4::operator +(const RTMatrix4x4& mat) const
+{
+ RTMatrix4x4 result = *this;
+ result += mat;
+ return result;
+}
+
+const RTMatrix4x4 RTMatrix4x4::operator *(const RTFLOAT val) const
+{
+ RTMatrix4x4 result = *this;
+ result *= val;
+ return result;
+}
+
+
+const RTMatrix4x4 RTMatrix4x4::operator *(const RTMatrix4x4& mat) const
+{
+ RTMatrix4x4 res;
+
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ res.m_data[row][col] =
+ m_data[row][0] * mat.m_data[0][col] +
+ m_data[row][1] * mat.m_data[1][col] +
+ m_data[row][2] * mat.m_data[2][col] +
+ m_data[row][3] * mat.m_data[3][col];
+
+ return res;
+}
+
+
+const RTQuaternion RTMatrix4x4::operator *(const RTQuaternion& q) const
+{
+ RTQuaternion res;
+
+ res.setScalar(m_data[0][0] * q.scalar() + m_data[0][1] * q.x() + m_data[0][2] * q.y() + m_data[0][3] * q.z());
+ res.setX(m_data[1][0] * q.scalar() + m_data[1][1] * q.x() + m_data[1][2] * q.y() + m_data[1][3] * q.z());
+ res.setY(m_data[2][0] * q.scalar() + m_data[2][1] * q.x() + m_data[2][2] * q.y() + m_data[2][3] * q.z());
+ res.setZ(m_data[3][0] * q.scalar() + m_data[3][1] * q.x() + m_data[3][2] * q.y() + m_data[3][3] * q.z());
+
+ return res;
+}
+
+void RTMatrix4x4::setToIdentity()
+{
+ fill(0);
+ m_data[0][0] = 1;
+ m_data[1][1] = 1;
+ m_data[2][2] = 1;
+ m_data[3][3] = 1;
+}
+
+RTMatrix4x4 RTMatrix4x4::transposed()
+{
+ RTMatrix4x4 res;
+
+ for (int row = 0; row < 4; row++)
+ for (int col = 0; col < 4; col++)
+ res.m_data[col][row] = m_data[row][col];
+ return res;
+}
+
+// Note:
+// The matrix inversion code here was strongly influenced by some old code I found
+// but I have no idea where it came from. Apologies to whoever wrote it originally!
+// If it's you, please let me know at info@richards-tech.com so I can credit it correctly.
+
+RTMatrix4x4 RTMatrix4x4::inverted()
+{
+ RTMatrix4x4 res;
+
+ RTFLOAT det = matDet();
+
+ if (det == 0) {
+ res.setToIdentity();
+ return res;
+ }
+
+ for (int row = 0; row < 4; row++) {
+ for (int col = 0; col < 4; col++) {
+ if ((row + col) & 1)
+ res.m_data[col][row] = -matMinor(row, col) / det;
+ else
+ res.m_data[col][row] = matMinor(row, col) / det;
+ }
+ }
+
+ return res;
+}
+
+RTFLOAT RTMatrix4x4::matDet()
+{
+ RTFLOAT det = 0;
+
+ det += m_data[0][0] * matMinor(0, 0);
+ det -= m_data[0][1] * matMinor(0, 1);
+ det += m_data[0][2] * matMinor(0, 2);
+ det -= m_data[0][3] * matMinor(0, 3);
+ return det;
+}
+
+RTFLOAT RTMatrix4x4::matMinor(const int row, const int col)
+{
+ static int map[] = {1, 2, 3, 0, 2, 3, 0, 1, 3, 0, 1, 2};
+
+ int *rc;
+ int *cc;
+ RTFLOAT res = 0;
+
+ rc = map + row * 3;
+ cc = map + col * 3;
+
+ res += m_data[rc[0]][cc[0]] * m_data[rc[1]][cc[1]] * m_data[rc[2]][cc[2]];
+ res -= m_data[rc[0]][cc[0]] * m_data[rc[1]][cc[2]] * m_data[rc[2]][cc[1]];
+ res -= m_data[rc[0]][cc[1]] * m_data[rc[1]][cc[0]] * m_data[rc[2]][cc[2]];
+ res += m_data[rc[0]][cc[1]] * m_data[rc[1]][cc[2]] * m_data[rc[2]][cc[0]];
+ res += m_data[rc[0]][cc[2]] * m_data[rc[1]][cc[0]] * m_data[rc[2]][cc[1]];
+ res -= m_data[rc[0]][cc[2]] * m_data[rc[1]][cc[1]] * m_data[rc[2]][cc[0]];
+ return res;
+}
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTMath.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTMath.h b/thirdparty/RTIMULib/RTIMULib/RTMath.h
new file mode 100644
index 0000000..d3e638d
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTMath.h
@@ -0,0 +1,195 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTMATH_H_
+#define _RTMATH_H_
+
+#include "RTIMUHal.h"
+
+// The fundamental float type
+
+#ifdef RTMATH_USE_DOUBLE
+typedef double RTFLOAT;
+#else
+typedef float RTFLOAT;
+#endif
+
+// Useful constants
+
+#define RTMATH_PI 3.1415926535
+#define RTMATH_DEGREE_TO_RAD (RTMATH_PI / 180.0)
+#define RTMATH_RAD_TO_DEGREE (180.0 / RTMATH_PI)
+
+class RTVector3;
+class RTMatrix4x4;
+class RTQuaternion;
+
+class RTMath
+{
+public:
+ // convenient display routines
+
+ static const char *displayRadians(const char *label, RTVector3& vec);
+ static const char *displayDegrees(const char *label, RTVector3& vec);
+ static const char *display(const char *label, RTQuaternion& quat);
+ static const char *display(const char *label, RTMatrix4x4& mat);
+
+ // currentUSecsSinceEpoch() is the source of all timestamps and
+ // is the number of uS since the standard epoch
+
+ static uint64_t currentUSecsSinceEpoch();
+
+ // poseFromAccelMag generates pose Euler angles from measured settings
+
+ static RTVector3 poseFromAccelMag(const RTVector3& accel, const RTVector3& mag);
+
+ // Takes signed 16 bit data from a char array and converts it to a vector of scaled RTFLOATs
+
+ static void convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian);
+
+ // Takes a pressure in hPa and returns height above sea level in meters
+
+ static RTFLOAT convertPressureToHeight(RTFLOAT pressure, RTFLOAT staticPressure = 1013.25);
+
+private:
+ static char m_string[1000]; // for the display routines
+};
+
+
+class RTVector3
+{
+public:
+ RTVector3();
+ RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z);
+
+ const RTVector3& operator +=(RTVector3& vec);
+ const RTVector3& operator -=(RTVector3& vec);
+
+ RTVector3& operator =(const RTVector3& vec);
+
+ RTFLOAT length();
+ void normalize();
+ void zero();
+ const char *display();
+ const char *displayDegrees();
+
+ static float dotProduct(const RTVector3& a, const RTVector3& b);
+ static void crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d);
+
+ void accelToEuler(RTVector3& rollPitchYaw) const;
+ void accelToQuaternion(RTQuaternion& qPose) const;
+
+ inline RTFLOAT x() const { return m_data[0]; }
+ inline RTFLOAT y() const { return m_data[1]; }
+ inline RTFLOAT z() const { return m_data[2]; }
+ inline RTFLOAT data(const int i) const { return m_data[i]; }
+
+ inline void setX(const RTFLOAT val) { m_data[0] = val; }
+ inline void setY(const RTFLOAT val) { m_data[1] = val; }
+ inline void setZ(const RTFLOAT val) { m_data[2] = val; }
+ inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
+ inline void fromArray(RTFLOAT *val) { memcpy(m_data, val, 3 * sizeof(RTFLOAT)); }
+ inline void toArray(RTFLOAT *val) const { memcpy(val, m_data, 3 * sizeof(RTFLOAT)); }
+
+private:
+ RTFLOAT m_data[3];
+};
+
+
+class RTQuaternion
+{
+public:
+ RTQuaternion();
+ RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z);
+
+ RTQuaternion& operator +=(const RTQuaternion& quat);
+ RTQuaternion& operator -=(const RTQuaternion& quat);
+ RTQuaternion& operator *=(const RTQuaternion& qb);
+ RTQuaternion& operator *=(const RTFLOAT val);
+ RTQuaternion& operator -=(const RTFLOAT val);
+
+ RTQuaternion& operator =(const RTQuaternion& quat);
+ const RTQuaternion operator *(const RTQuaternion& qb) const;
+ const RTQuaternion operator *(const RTFLOAT val) const;
+ const RTQuaternion operator -(const RTQuaternion& qb) const;
+ const RTQuaternion operator -(const RTFLOAT val) const;
+
+ void normalize();
+ void toEuler(RTVector3& vec);
+ void fromEuler(RTVector3& vec);
+ RTQuaternion conjugate() const;
+ void toAngleVector(RTFLOAT& angle, RTVector3& vec);
+ void fromAngleVector(const RTFLOAT& angle, const RTVector3& vec);
+
+ void zero();
+ const char *display();
+
+ inline RTFLOAT scalar() const { return m_data[0]; }
+ inline RTFLOAT x() const { return m_data[1]; }
+ inline RTFLOAT y() const { return m_data[2]; }
+ inline RTFLOAT z() const { return m_data[3]; }
+ inline RTFLOAT data(const int i) const { return m_data[i]; }
+
+ inline void setScalar(const RTFLOAT val) { m_data[0] = val; }
+ inline void setX(const RTFLOAT val) { m_data[1] = val; }
+ inline void setY(const RTFLOAT val) { m_data[2] = val; }
+ inline void setZ(const RTFLOAT val) { m_data[3] = val; }
+ inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
+ inline void fromArray(RTFLOAT *val) { memcpy(m_data, val, 4 * sizeof(RTFLOAT)); }
+ inline void toArray(RTFLOAT *val) const { memcpy(val, m_data, 4 * sizeof(RTFLOAT)); }
+
+private:
+ RTFLOAT m_data[4];
+};
+
+class RTMatrix4x4
+{
+public:
+ RTMatrix4x4();
+
+ RTMatrix4x4& operator +=(const RTMatrix4x4& mat);
+ RTMatrix4x4& operator -=(const RTMatrix4x4& mat);
+ RTMatrix4x4& operator *=(const RTFLOAT val);
+
+ RTMatrix4x4& operator =(const RTMatrix4x4& vec);
+ const RTQuaternion operator *(const RTQuaternion& q) const;
+ const RTMatrix4x4 operator *(const RTFLOAT val) const;
+ const RTMatrix4x4 operator *(const RTMatrix4x4& mat) const;
+ const RTMatrix4x4 operator +(const RTMatrix4x4& mat) const;
+
+ inline RTFLOAT val(int row, int col) const { return m_data[row][col]; }
+ inline void setVal(int row, int col, RTFLOAT val) { m_data[row][col] = val; }
+ void fill(RTFLOAT val);
+ void setToIdentity();
+
+ RTMatrix4x4 inverted();
+ RTMatrix4x4 transposed();
+
+private:
+ RTFLOAT m_data[4][4]; // row, column
+
+ RTFLOAT matDet();
+ RTFLOAT matMinor(const int row, const int col);
+};
+
+#endif /* _RTMATH_H_ */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULibVersion.txt
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULibVersion.txt b/thirdparty/RTIMULib/RTIMULibVersion.txt
new file mode 100644
index 0000000..3fdb3eb
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULibVersion.txt
@@ -0,0 +1,6 @@
+SET(RTIMULIB_VERSION_MAJOR 7)
+SET(RTIMULIB_VERSION_MINOR 2)
+SET(RTIMULIB_VERSION_PATCH 1)
+SET(RTIMULIB_VERSION
+${RTIMULIB_VERSION_MAJOR}.${RTIMULIB_VERSION_MINOR}.${RTIMULIB_VERSION_PATCH})
+
[3/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
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
+// staslock@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
+// staslock@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
+// staslock@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
+// staslock@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
[2/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.cpp b/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.cpp
new file mode 100644
index 0000000..783dbff
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.cpp
@@ -0,0 +1,1723 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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
+// staslock@gmail.com (www.clickdrive.io)
+
+
+#include "RTIMUSettings.h"
+#include "IMUDrivers/RTIMUMPU9150.h"
+#include "IMUDrivers/RTIMUMPU9250.h"
+#include "IMUDrivers/RTIMUGD20HM303D.h"
+#include "IMUDrivers/RTIMUGD20M303DLHC.h"
+#include "IMUDrivers/RTIMUGD20HM303DLHC.h"
+#include "IMUDrivers/RTIMULSM9DS0.h"
+#include "IMUDrivers/RTIMULSM9DS1.h"
+#include "IMUDrivers/RTIMUBMX055.h"
+
+#include "IMUDrivers/RTPressureBMP180.h"
+#include "IMUDrivers/RTPressureLPS25H.h"
+
+#include "IMUDrivers/RTHumidityHTS221.h"
+#include "IMUDrivers/RTHumidityHTU21D.h"
+
+#define RATE_TIMER_INTERVAL 2
+
+RTIMUSettings::RTIMUSettings(const char *productType)
+{
+ if ((strlen(productType) > 200) || (strlen(productType) == 0)) {
+ HAL_ERROR("Product name too long or null - using default\n");
+ strcpy(m_filename, "RTIMULib.ini");
+ } else {
+ sprintf(m_filename, "%s.ini", productType);
+ }
+ loadSettings();
+}
+
+RTIMUSettings::RTIMUSettings(const char *settingsDirectory, const char *productType)
+{
+ if (((strlen(productType) + strlen(settingsDirectory)) > 200) || (strlen(productType) == 0)) {
+ HAL_ERROR("Product name too long or null - using default\n");
+ strcpy(m_filename, "RTIMULib.ini");
+ } else {
+ sprintf(m_filename, "%s/%s.ini", settingsDirectory, productType);
+ }
+ loadSettings();
+}
+
+
+bool RTIMUSettings::discoverIMU(int& imuType, bool& busIsI2C, unsigned char& slaveAddress)
+{
+ unsigned char result;
+ unsigned char altResult;
+
+ // auto detect on I2C bus
+
+ m_busIsI2C = true;
+
+ if (HALOpen()) {
+
+ if (HALRead(MPU9150_ADDRESS0, MPU9150_WHO_AM_I, 1, &result, "")) {
+ if (result == MPU9250_ID) {
+ imuType = RTIMU_TYPE_MPU9250;
+ slaveAddress = MPU9250_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected MPU9250 at standard address\n");
+ return true;
+ } else if (result == MPU9150_ID) {
+ imuType = RTIMU_TYPE_MPU9150;
+ slaveAddress = MPU9150_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected MPU9150 at standard address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(MPU9150_ADDRESS1, MPU9150_WHO_AM_I, 1, &result, "")) {
+ if (result == MPU9250_ID) {
+ imuType = RTIMU_TYPE_MPU9250;
+ slaveAddress = MPU9250_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected MPU9250 at option address\n");
+ return true;
+ } else if (result == MPU9150_ID) {
+ imuType = RTIMU_TYPE_MPU9150;
+ slaveAddress = MPU9150_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected MPU9150 at option address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(L3GD20H_ADDRESS0, L3GD20H_WHO_AM_I, 1, &result, "")) {
+ if (result == L3GD20H_ID) {
+ if (HALRead(LSM303D_ADDRESS0, LSM303D_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM303D_ID) {
+ imuType = RTIMU_TYPE_GD20HM303D;
+ slaveAddress = L3GD20H_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303D at standard/standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM303D_ADDRESS1, LSM303D_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM303D_ID) {
+ imuType = RTIMU_TYPE_GD20HM303D;
+ slaveAddress = L3GD20H_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303D at standard/option address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_A, 1, &altResult, "")) {
+ imuType = RTIMU_TYPE_GD20HM303DLHC;
+ slaveAddress = L3GD20H_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303DLHC at standard/standard address\n");
+ return true;
+ }
+ } else if (result == LSM9DS0_GYRO_ID) {
+ if (HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS0_ACCELMAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS0;
+ slaveAddress = LSM9DS0_GYRO_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS0 at standard/standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS0_ACCELMAG_ADDRESS1, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS0_ACCELMAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS0;
+ slaveAddress = LSM9DS0_GYRO_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS0 at standard/option address\n");
+ return true;
+ }
+ }
+ } else if (result == LSM9DS1_ID) {
+ if (HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at standard/standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at standard/option 1 address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at standard/option 2 address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at standard/option 3 address\n");
+ return true;
+ }
+ }
+ }
+ }
+
+ if (HALRead(L3GD20H_ADDRESS1, L3GD20H_WHO_AM_I, 1, &result, "")) {
+ if (result == L3GD20H_ID) {
+ if (HALRead(LSM303D_ADDRESS1, LSM303D_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM303D_ID) {
+ imuType = RTIMU_TYPE_GD20HM303D;
+ slaveAddress = L3GD20H_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303D at option/option address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM303D_ADDRESS0, LSM303D_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM303D_ID) {
+ imuType = RTIMU_TYPE_GD20HM303D;
+ slaveAddress = L3GD20H_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303D at option/standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_A, 1, &altResult, "")) {
+ imuType = RTIMU_TYPE_GD20HM303DLHC;
+ slaveAddress = L3GD20H_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20H/LSM303DLHC at option/standard address\n");
+ return true;
+ }
+ } else if (result == LSM9DS0_GYRO_ID) {
+ if (HALRead(LSM9DS0_ACCELMAG_ADDRESS1, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS0_ACCELMAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS0;
+ slaveAddress = LSM9DS0_GYRO_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS0 at option/option address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS0_ACCELMAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS0;
+ slaveAddress = LSM9DS0_GYRO_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS0 at option/standard address\n");
+ return true;
+ }
+ }
+ } else if (result == LSM9DS1_ID) {
+ if (HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at option/standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at option/option 1 address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at option/option 2 address\n");
+ return true;
+ }
+ }
+ if (HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &altResult, "")) {
+ if (altResult == LSM9DS1_MAG_ID) {
+ imuType = RTIMU_TYPE_LSM9DS1;
+ slaveAddress = LSM9DS1_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected LSM9DS1 at option/option 3 address\n");
+ return true;
+ }
+ }
+ }
+ }
+
+ if (HALRead(L3GD20_ADDRESS0, L3GD20_WHO_AM_I, 1, &result, "")) {
+ if (result == L3GD20_ID) {
+ imuType = RTIMU_TYPE_GD20M303DLHC;
+ slaveAddress = L3GD20_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20 at standard address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(L3GD20_ADDRESS1, L3GD20_WHO_AM_I, 1, &result, "")) {
+ if (result == L3GD20_ID) {
+ imuType = RTIMU_TYPE_GD20M303DLHC;
+ slaveAddress = L3GD20_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected L3GD20 at option address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(BMX055_GYRO_ADDRESS0, BMX055_GYRO_WHO_AM_I, 1, &result, "")) {
+ if (result == BMX055_GYRO_ID) {
+ imuType = RTIMU_TYPE_BMX055;
+ slaveAddress = BMX055_GYRO_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected BMX055 at standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(BMX055_GYRO_ADDRESS1, BMX055_GYRO_WHO_AM_I, 1, &result, "")) {
+ if (result == BMX055_GYRO_ID) {
+ imuType = RTIMU_TYPE_BMX055;
+ slaveAddress = BMX055_GYRO_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected BMX055 at option address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(BNO055_ADDRESS0, BNO055_WHO_AM_I, 1, &result, "")) {
+ if (result == BNO055_ID) {
+ imuType = RTIMU_TYPE_BNO055;
+ slaveAddress = BNO055_ADDRESS0;
+ busIsI2C = true;
+ HAL_INFO("Detected BNO055 at standard address\n");
+ return true;
+ }
+ }
+ if (HALRead(BNO055_ADDRESS1, BNO055_WHO_AM_I, 1, &result, "")) {
+ if (result == BNO055_ID) {
+ imuType = RTIMU_TYPE_BNO055;
+ slaveAddress = BNO055_ADDRESS1;
+ busIsI2C = true;
+ HAL_INFO("Detected BNO055 at option address\n");
+ return true;
+ }
+ }
+ HALClose();
+ }
+
+ // nothing found on I2C bus - try SPI instead
+
+ m_busIsI2C = false;
+ m_SPIBus = 0;
+
+ m_SPISelect = 0;
+
+ if (HALOpen()) {
+ if (HALRead(MPU9250_ADDRESS0, MPU9250_WHO_AM_I, 1, &result, "")) {
+ if (result == MPU9250_ID) {
+ imuType = RTIMU_TYPE_MPU9250;
+ slaveAddress = MPU9250_ADDRESS0;
+ busIsI2C = false;
+ HAL_INFO("Detected MPU9250 on SPI bus 0, select 0\n");
+ return true;
+ }
+ }
+ HALClose();
+ }
+
+ m_SPISelect = 1;
+
+ if (HALOpen()) {
+ if (HALRead(MPU9250_ADDRESS0, MPU9250_WHO_AM_I, 1, &result, "")) {
+ if (result == MPU9250_ID) {
+ imuType = RTIMU_TYPE_MPU9250;
+ slaveAddress = MPU9250_ADDRESS0;
+ busIsI2C = false;
+ HAL_INFO("Detected MPU9250 on SPI bus 0, select 1\n");
+ return true;
+ }
+ }
+ HALClose();
+ }
+
+ HAL_ERROR("No IMU detected\n");
+ return false;
+}
+
+bool RTIMUSettings::discoverPressure(int& pressureType, unsigned char& pressureAddress)
+{
+ unsigned char result;
+
+ // auto detect on current bus
+
+ if (HALOpen()) {
+
+ if (HALRead(BMP180_ADDRESS, BMP180_REG_ID, 1, &result, "")) {
+ if (result == BMP180_ID) {
+ pressureType = RTPRESSURE_TYPE_BMP180;
+ pressureAddress = BMP180_ADDRESS;
+ HAL_INFO("Detected BMP180\n");
+ return true;
+ }
+ }
+
+ if (HALRead(LPS25H_ADDRESS0, LPS25H_REG_ID, 1, &result, "")) {
+ if (result == LPS25H_ID) {
+ pressureType = RTPRESSURE_TYPE_LPS25H;
+ pressureAddress = LPS25H_ADDRESS0;
+ HAL_INFO("Detected LPS25H at standard address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(LPS25H_ADDRESS1, LPS25H_REG_ID, 1, &result, "")) {
+ if (result == LPS25H_ID) {
+ pressureType = RTPRESSURE_TYPE_LPS25H;
+ pressureAddress = LPS25H_ADDRESS1;
+ HAL_INFO("Detected LPS25H at option address\n");
+ return true;
+ }
+ }
+
+ // check for MS5611 (which unfortunately has no ID reg)
+
+ if (HALRead(MS5611_ADDRESS0, 0, 1, &result, "")) {
+ pressureType = RTPRESSURE_TYPE_MS5611;
+ pressureAddress = MS5611_ADDRESS0;
+ HAL_INFO("Detected MS5611 at standard address\n");
+ return true;
+ }
+ if (HALRead(MS5611_ADDRESS1, 0, 1, &result, "")) {
+ pressureType = RTPRESSURE_TYPE_MS5611;
+ pressureAddress = MS5611_ADDRESS1;
+ HAL_INFO("Detected MS5611 at option address\n");
+ return true;
+ }
+ }
+ HAL_ERROR("No pressure sensor detected\n");
+ return false;
+}
+
+bool RTIMUSettings::discoverHumidity(int& humidityType, unsigned char& humidityAddress)
+{
+ unsigned char result;
+
+ // auto detect on current bus
+
+ if (HALOpen()) {
+
+ if (HALRead(HTS221_ADDRESS, HTS221_REG_ID, 1, &result, "")) {
+ if (result == HTS221_ID) {
+ humidityType = RTHUMIDITY_TYPE_HTS221;
+ humidityAddress = HTS221_ADDRESS;
+ HAL_INFO("Detected HTS221 at standard address\n");
+ return true;
+ }
+ }
+
+ if (HALRead(HTU21D_ADDRESS, HTU21D_READ_USER_REG, 1, &result, "")) {
+ humidityType = RTHUMIDITY_TYPE_HTU21D;
+ humidityAddress = HTU21D_ADDRESS;
+ HAL_INFO("Detected HTU21D at standard address\n");
+ return true;
+ }
+
+ }
+ HAL_ERROR("No humidity sensor detected\n");
+ return false;
+}
+
+void RTIMUSettings::setDefaults()
+{
+ // preset general defaults
+
+ m_imuType = RTIMU_TYPE_AUTODISCOVER;
+ m_I2CSlaveAddress = 0;
+ m_busIsI2C = true;
+ m_I2CBus = 1;
+ m_SPIBus = 0;
+ m_SPISelect = 0;
+ m_SPISpeed = 500000;
+ m_fusionType = RTFUSION_TYPE_RTQF;
+ m_axisRotation = RTIMU_XNORTH_YEAST;
+ m_pressureType = RTPRESSURE_TYPE_AUTODISCOVER;
+ m_I2CPressureAddress = 0;
+ m_humidityType = RTHUMIDITY_TYPE_AUTODISCOVER;
+ m_I2CHumidityAddress = 0;
+ m_compassCalValid = false;
+ m_compassCalEllipsoidValid = false;
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ m_compassCalEllipsoidCorr[i][j] = 0;
+ }
+ }
+ m_compassCalEllipsoidCorr[0][0] = 1;
+ m_compassCalEllipsoidCorr[1][1] = 1;
+ m_compassCalEllipsoidCorr[2][2] = 1;
+
+ m_compassAdjDeclination = 0;
+
+ m_accelCalValid = false;
+ m_gyroBiasValid = false;
+
+ // MPU9150 defaults
+
+ m_MPU9150GyroAccelSampleRate = 50;
+ m_MPU9150CompassSampleRate = 25;
+ m_MPU9150GyroAccelLpf = MPU9150_LPF_20;
+ m_MPU9150GyroFsr = MPU9150_GYROFSR_1000;
+ m_MPU9150AccelFsr = MPU9150_ACCELFSR_8;
+
+ // MPU9250 defaults
+
+ m_MPU9250GyroAccelSampleRate = 80;
+ m_MPU9250CompassSampleRate = 40;
+ m_MPU9250GyroLpf = MPU9250_GYRO_LPF_41;
+ m_MPU9250AccelLpf = MPU9250_ACCEL_LPF_41;
+ m_MPU9250GyroFsr = MPU9250_GYROFSR_1000;
+ m_MPU9250AccelFsr = MPU9250_ACCELFSR_8;
+
+ // GD20HM303D defaults
+
+ m_GD20HM303DGyroSampleRate = L3GD20H_SAMPLERATE_50;
+ m_GD20HM303DGyroBW = L3GD20H_BANDWIDTH_1;
+ m_GD20HM303DGyroHpf = L3GD20H_HPF_4;
+ m_GD20HM303DGyroFsr = L3GD20H_FSR_500;
+
+ m_GD20HM303DAccelSampleRate = LSM303D_ACCEL_SAMPLERATE_50;
+ m_GD20HM303DAccelFsr = LSM303D_ACCEL_FSR_8;
+ m_GD20HM303DAccelLpf = LSM303D_ACCEL_LPF_50;
+
+ m_GD20HM303DCompassSampleRate = LSM303D_COMPASS_SAMPLERATE_50;
+ m_GD20HM303DCompassFsr = LSM303D_COMPASS_FSR_2;
+
+ // GD20M303DLHC defaults
+
+ m_GD20M303DLHCGyroSampleRate = L3GD20_SAMPLERATE_95;
+ m_GD20M303DLHCGyroBW = L3GD20_BANDWIDTH_1;
+ m_GD20M303DLHCGyroHpf = L3GD20_HPF_4;
+ m_GD20M303DLHCGyroFsr = L3GD20H_FSR_500;
+
+ m_GD20M303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+ m_GD20M303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+ m_GD20M303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+ m_GD20M303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+ // GD20HM303DLHC defaults
+
+ m_GD20HM303DLHCGyroSampleRate = L3GD20H_SAMPLERATE_50;
+ m_GD20HM303DLHCGyroBW = L3GD20H_BANDWIDTH_1;
+ m_GD20HM303DLHCGyroHpf = L3GD20H_HPF_4;
+ m_GD20HM303DLHCGyroFsr = L3GD20H_FSR_500;
+
+ m_GD20HM303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+ m_GD20HM303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+ m_GD20HM303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+ m_GD20HM303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+ // LSM9DS0 defaults
+
+ m_LSM9DS0GyroSampleRate = LSM9DS0_GYRO_SAMPLERATE_95;
+ m_LSM9DS0GyroBW = LSM9DS0_GYRO_BANDWIDTH_1;
+ m_LSM9DS0GyroHpf = LSM9DS0_GYRO_HPF_4;
+ m_LSM9DS0GyroFsr = LSM9DS0_GYRO_FSR_500;
+
+ m_LSM9DS0AccelSampleRate = LSM9DS0_ACCEL_SAMPLERATE_50;
+ m_LSM9DS0AccelFsr = LSM9DS0_ACCEL_FSR_8;
+ m_LSM9DS0AccelLpf = LSM9DS0_ACCEL_LPF_50;
+
+ m_LSM9DS0CompassSampleRate = LSM9DS0_COMPASS_SAMPLERATE_50;
+ m_LSM9DS0CompassFsr = LSM9DS0_COMPASS_FSR_2;
+
+ // LSM9DS1 defaults
+
+ m_LSM9DS1GyroSampleRate = LSM9DS1_GYRO_SAMPLERATE_119;
+ m_LSM9DS1GyroBW = LSM9DS1_GYRO_BANDWIDTH_1;
+ m_LSM9DS1GyroHpf = LSM9DS1_GYRO_HPF_4;
+ m_LSM9DS1GyroFsr = LSM9DS1_GYRO_FSR_500;
+
+ m_LSM9DS1AccelSampleRate = LSM9DS1_ACCEL_SAMPLERATE_119;
+ m_LSM9DS1AccelFsr = LSM9DS1_ACCEL_FSR_8;
+ m_LSM9DS1AccelLpf = LSM9DS1_ACCEL_LPF_50;
+
+ m_LSM9DS1CompassSampleRate = LSM9DS1_COMPASS_SAMPLERATE_20;
+ m_LSM9DS1CompassFsr = LSM9DS1_COMPASS_FSR_4;
+ // BMX055 defaults
+
+ m_BMX055GyroSampleRate = BMX055_GYRO_SAMPLERATE_100_32;
+ m_BMX055GyroFsr = BMX055_GYRO_FSR_500;
+
+ m_BMX055AccelSampleRate = BMX055_ACCEL_SAMPLERATE_125;
+ m_BMX055AccelFsr = BMX055_ACCEL_FSR_8;
+
+ m_BMX055MagPreset = BMX055_MAG_REGULAR;
+}
+
+bool RTIMUSettings::loadSettings()
+{
+ setDefaults();
+
+ char buf[200];
+ char key[200];
+ char val[200];
+ RTFLOAT ftemp;
+ // check to see if settings file exists
+
+ if (!(m_fd = fopen(m_filename, "r"))) {
+ HAL_INFO("Settings file not found. Using defaults and creating settings file\n");
+ return saveSettings();
+ }
+
+ while (fgets(buf, 200, m_fd)) {
+ if ((buf[0] == '#') || (buf[0] == ' ') || (buf[0] == '\n'))
+ // just a comment
+ continue;
+
+ if (sscanf(buf, "%[^=]=%s", key, val) != 2) {
+ HAL_ERROR1("Bad line in settings file: %s\n", buf);
+ fclose(m_fd);
+ return false;
+ }
+
+ // now decode keys
+
+ // general config
+
+ if (strcmp(key, RTIMULIB_IMU_TYPE) == 0) {
+ m_imuType = atoi(val);
+ } else if (strcmp(key, RTIMULIB_FUSION_TYPE) == 0) {
+ m_fusionType = atoi(val);
+ } else if (strcmp(key, RTIMULIB_BUS_IS_I2C) == 0) {
+ m_busIsI2C = strcmp(val, "true") == 0;
+ } else if (strcmp(key, RTIMULIB_I2C_BUS) == 0) {
+ m_I2CBus = atoi(val);
+ } else if (strcmp(key, RTIMULIB_SPI_BUS) == 0) {
+ m_SPIBus = atoi(val);
+ } else if (strcmp(key, RTIMULIB_SPI_SELECT) == 0) {
+ m_SPISelect = atoi(val);
+ } else if (strcmp(key, RTIMULIB_SPI_SPEED) == 0) {
+ m_SPISpeed = atoi(val);
+ } else if (strcmp(key, RTIMULIB_I2C_SLAVEADDRESS) == 0) {
+ m_I2CSlaveAddress = atoi(val);
+ } else if (strcmp(key, RTIMULIB_AXIS_ROTATION) == 0) {
+ m_axisRotation = atoi(val);
+ } else if (strcmp(key, RTIMULIB_PRESSURE_TYPE) == 0) {
+ m_pressureType = atoi(val);
+ } else if (strcmp(key, RTIMULIB_I2C_PRESSUREADDRESS) == 0) {
+ m_I2CPressureAddress = atoi(val);
+ } else if (strcmp(key, RTIMULIB_HUMIDITY_TYPE) == 0) {
+ m_humidityType = atoi(val);
+ } else if (strcmp(key, RTIMULIB_I2C_HUMIDITYADDRESS) == 0) {
+ m_I2CHumidityAddress = atoi(val);
+
+ // compass calibration and adjustment
+
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_VALID) == 0) {
+ m_compassCalValid = strcmp(val, "true") == 0;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINX) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMin.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINY) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMin.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINZ) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMin.setZ(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXX) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMax.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXY) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMax.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXZ) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalMax.setZ(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSADJ_DECLINATION) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassAdjDeclination = ftemp;
+
+ // compass ellipsoid calibration
+
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_ELLIPSOID_VALID) == 0) {
+ m_compassCalEllipsoidValid = strcmp(val, "true") == 0;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_OFFSET_X) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidOffset.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_OFFSET_Y) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidOffset.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_OFFSET_Z) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidOffset.setZ(ftemp);
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR11) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[0][0] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR12) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[0][1] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR13) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[0][2] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR21) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[1][0] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR22) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[1][1] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR23) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[1][2] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR31) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[2][0] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR32) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[2][1] = ftemp;
+ } else if (strcmp(key, RTIMULIB_COMPASSCAL_CORR33) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_compassCalEllipsoidCorr[2][2] = ftemp;
+
+ // accel calibration
+
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_VALID) == 0) {
+ m_accelCalValid = strcmp(val, "true") == 0;
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MINX) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMin.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MINY) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMin.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MINZ) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMin.setZ(ftemp);
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MAXX) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMax.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MAXY) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMax.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_ACCELCAL_MAXZ) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_accelCalMax.setZ(ftemp);
+
+ // gyro bias
+
+ } else if (strcmp(key, RTIMULIB_GYRO_BIAS_VALID) == 0) {
+ m_gyroBiasValid = strcmp(val, "true") == 0;
+ } else if (strcmp(key, RTIMULIB_GYRO_BIAS_X) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_gyroBias.setX(ftemp);
+ } else if (strcmp(key, RTIMULIB_GYRO_BIAS_Y) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_gyroBias.setY(ftemp);
+ } else if (strcmp(key, RTIMULIB_GYRO_BIAS_Z) == 0) {
+ sscanf(val, "%f", &ftemp);
+ m_gyroBias.setZ(ftemp);
+
+ // MPU9150 settings
+
+ } else if (strcmp(key, RTIMULIB_MPU9150_GYROACCEL_SAMPLERATE) == 0) {
+ m_MPU9150GyroAccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9150_COMPASS_SAMPLERATE) == 0) {
+ m_MPU9150CompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9150_GYROACCEL_LPF) == 0) {
+ m_MPU9150GyroAccelLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9150_GYRO_FSR) == 0) {
+ m_MPU9150GyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9150_ACCEL_FSR) == 0) {
+ m_MPU9150AccelFsr = atoi(val);
+
+ // MPU9250 settings
+
+ } else if (strcmp(key, RTIMULIB_MPU9250_GYROACCEL_SAMPLERATE) == 0) {
+ m_MPU9250GyroAccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9250_COMPASS_SAMPLERATE) == 0) {
+ m_MPU9250CompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9250_GYRO_LPF) == 0) {
+ m_MPU9250GyroLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9250_ACCEL_LPF) == 0) {
+ m_MPU9250AccelLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9250_GYRO_FSR) == 0) {
+ m_MPU9250GyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_MPU9250_ACCEL_FSR) == 0) {
+ m_MPU9250AccelFsr = atoi(val);
+
+ // GD20HM303D settings
+
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_SAMPLERATE) == 0) {
+ m_GD20HM303DGyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_FSR) == 0) {
+ m_GD20HM303DGyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_HPF) == 0) {
+ m_GD20HM303DGyroHpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_BW) == 0) {
+ m_GD20HM303DGyroBW = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_SAMPLERATE) == 0) {
+ m_GD20HM303DAccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_FSR) == 0) {
+ m_GD20HM303DAccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_LPF) == 0) {
+ m_GD20HM303DAccelLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_COMPASS_SAMPLERATE) == 0) {
+ m_GD20HM303DCompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303D_COMPASS_FSR) == 0) {
+ m_GD20HM303DCompassFsr = atoi(val);
+
+ // GD20M303DLHC settings
+
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_SAMPLERATE) == 0) {
+ m_GD20M303DLHCGyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_FSR) == 0) {
+ m_GD20M303DLHCGyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_HPF) == 0) {
+ m_GD20M303DLHCGyroHpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_BW) == 0) {
+ m_GD20M303DLHCGyroBW = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_ACCEL_SAMPLERATE) == 0) {
+ m_GD20M303DLHCAccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_ACCEL_FSR) == 0) {
+ m_GD20M303DLHCAccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_COMPASS_SAMPLERATE) == 0) {
+ m_GD20M303DLHCCompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20M303DLHC_COMPASS_FSR) == 0) {
+ m_GD20M303DLHCCompassFsr = atoi(val);
+
+ // GD20HM303DLHC settings
+
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_GYRO_SAMPLERATE) == 0) {
+ m_GD20HM303DLHCGyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_GYRO_FSR) == 0) {
+ m_GD20HM303DLHCGyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_GYRO_HPF) == 0) {
+ m_GD20HM303DLHCGyroHpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_GYRO_BW) == 0) {
+ m_GD20HM303DLHCGyroBW = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_ACCEL_SAMPLERATE) == 0) {
+ m_GD20HM303DLHCAccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_ACCEL_FSR) == 0) {
+ m_GD20HM303DLHCAccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_COMPASS_SAMPLERATE) == 0) {
+ m_GD20HM303DLHCCompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_GD20HM303DLHC_COMPASS_FSR) == 0) {
+ m_GD20HM303DLHCCompassFsr = atoi(val);
+
+ // LSM9DS0 settings
+
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_SAMPLERATE) == 0) {
+ m_LSM9DS0GyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_FSR) == 0) {
+ m_LSM9DS0GyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_HPF) == 0) {
+ m_LSM9DS0GyroHpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_BW) == 0) {
+ m_LSM9DS0GyroBW = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_SAMPLERATE) == 0) {
+ m_LSM9DS0AccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_FSR) == 0) {
+ m_LSM9DS0AccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_LPF) == 0) {
+ m_LSM9DS0AccelLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_COMPASS_SAMPLERATE) == 0) {
+ m_LSM9DS0CompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS0_COMPASS_FSR) == 0) {
+ m_LSM9DS0CompassFsr = atoi(val);
+
+ // LSM9DS1 settings
+
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_GYRO_SAMPLERATE) == 0) {
+ m_LSM9DS1GyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_GYRO_FSR) == 0) {
+ m_LSM9DS1GyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_GYRO_HPF) == 0) {
+ m_LSM9DS1GyroHpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_GYRO_BW) == 0) {
+ m_LSM9DS1GyroBW = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_ACCEL_SAMPLERATE) == 0) {
+ m_LSM9DS1AccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_ACCEL_FSR) == 0) {
+ m_LSM9DS1AccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_ACCEL_LPF) == 0) {
+ m_LSM9DS1AccelLpf = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_COMPASS_SAMPLERATE) == 0) {
+ m_LSM9DS1CompassSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_LSM9DS1_COMPASS_FSR) == 0) {
+ m_LSM9DS1CompassFsr = atoi(val);
+
+ // BMX055 settings
+
+ } else if (strcmp(key, RTIMULIB_BMX055_GYRO_SAMPLERATE) == 0) {
+ m_BMX055GyroSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_BMX055_GYRO_FSR) == 0) {
+ m_BMX055GyroFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_BMX055_ACCEL_SAMPLERATE) == 0) {
+ m_BMX055AccelSampleRate = atoi(val);
+ } else if (strcmp(key, RTIMULIB_BMX055_ACCEL_FSR) == 0) {
+ m_BMX055AccelFsr = atoi(val);
+ } else if (strcmp(key, RTIMULIB_BMX055_MAG_PRESET) == 0) {
+ m_BMX055MagPreset = atoi(val);
+
+ // Handle unrecognized key
+
+ } else {
+ HAL_ERROR1("Unrecognized key in settings file: %s\n", buf);
+ }
+ }
+ HAL_INFO1("Settings file %s loaded\n", m_filename);
+ fclose(m_fd);
+ return saveSettings(); // make sure settings file is correct and complete
+}
+
+bool RTIMUSettings::saveSettings()
+{
+ if (!(m_fd = fopen(m_filename, "w"))) {
+ HAL_ERROR("Failed to open settings file for save");
+ return false;
+ }
+
+ // General settings
+
+ setComment("#####################################################################");
+ setComment("");
+ setComment("RTIMULib settings file");
+ setBlank();
+ setComment("General settings");
+ setComment("");
+
+ setBlank();
+ setComment("IMU type - ");
+ setComment(" 0 = Auto discover");
+ setComment(" 1 = Null (used when data is provided from a remote IMU");
+ setComment(" 2 = InvenSense MPU-9150");
+ setComment(" 3 = STM L3GD20H + LSM303D");
+ setComment(" 4 = STM L3GD20 + LSM303DLHC");
+ setComment(" 5 = STM LSM9DS0");
+ setComment(" 6 = STM LSM9DS1");
+ setComment(" 7 = InvenSense MPU-9250");
+ setComment(" 8 = STM L3GD20H + LSM303DLHC");
+ setComment(" 9 = Bosch BMX055");
+ setComment(" 10 = Bosch BNX055");
+ setValue(RTIMULIB_IMU_TYPE, m_imuType);
+
+ setBlank();
+ setComment("");
+ setComment("Fusion type type - ");
+ setComment(" 0 - Null. Use if only sensor data required without fusion");
+ setComment(" 1 - Kalman STATE4");
+ setComment(" 2 - RTQF");
+ setValue(RTIMULIB_FUSION_TYPE, m_fusionType);
+
+ setBlank();
+ setComment("");
+ setComment("Is bus I2C: 'true' for I2C, 'false' for SPI");
+ setValue(RTIMULIB_BUS_IS_I2C, m_busIsI2C);
+
+ setBlank();
+ setComment("");
+ setComment("I2C Bus (between 0 and 7) ");
+ setValue(RTIMULIB_I2C_BUS, m_I2CBus);
+
+ setBlank();
+ setComment("");
+ setComment("SPI Bus (between 0 and 7) ");
+ setValue(RTIMULIB_SPI_BUS, m_SPIBus);
+
+ setBlank();
+ setComment("");
+ setComment("SPI select (between 0 and 1) ");
+ setValue(RTIMULIB_SPI_SELECT, m_SPISelect);
+
+ setBlank();
+ setComment("");
+ setComment("SPI Speed in Hz");
+ setValue(RTIMULIB_SPI_SPEED, (int)m_SPISpeed);
+
+ setBlank();
+ setComment("");
+ setComment("I2C slave address (filled in automatically by auto discover) ");
+ setValue(RTIMULIB_I2C_SLAVEADDRESS, m_I2CSlaveAddress);
+
+ setBlank();
+ setComment("");
+ setComment("IMU axis rotation - see RTIMU.h for details");
+ setValue(RTIMULIB_AXIS_ROTATION, m_axisRotation);
+
+ setBlank();
+ setComment("Pressure sensor type - ");
+ setComment(" 0 = Auto discover");
+ setComment(" 1 = Null (no hardware or don't use)");
+ setComment(" 2 = BMP180");
+ setComment(" 3 = LPS25H");
+ setComment(" 4 = MS5611");
+ setComment(" 5 = MS5637");
+
+ setValue(RTIMULIB_PRESSURE_TYPE, m_pressureType);
+
+ setBlank();
+ setComment("");
+ setComment("I2C pressure sensor address (filled in automatically by auto discover) ");
+ setValue(RTIMULIB_I2C_PRESSUREADDRESS, m_I2CPressureAddress);
+
+ setBlank();
+ setComment("Humidity sensor type - ");
+ setComment(" 0 = Auto discover");
+ setComment(" 1 = Null (no hardware or don't use)");
+ setComment(" 2 = HTS221");
+ setComment(" 3 = HTU21D");
+
+ setValue(RTIMULIB_HUMIDITY_TYPE, m_humidityType);
+
+ setBlank();
+ setComment("");
+ setComment("I2C humidity sensor address (filled in automatically by auto discover) ");
+ setValue(RTIMULIB_I2C_HUMIDITYADDRESS, m_I2CHumidityAddress);
+
+ // Compass settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+
+ setBlank();
+ setComment("Compass calibration settings");
+ setValue(RTIMULIB_COMPASSCAL_VALID, m_compassCalValid);
+ setValue(RTIMULIB_COMPASSCAL_MINX, m_compassCalMin.x());
+ setValue(RTIMULIB_COMPASSCAL_MINY, m_compassCalMin.y());
+ setValue(RTIMULIB_COMPASSCAL_MINZ, m_compassCalMin.z());
+ setValue(RTIMULIB_COMPASSCAL_MAXX, m_compassCalMax.x());
+ setValue(RTIMULIB_COMPASSCAL_MAXY, m_compassCalMax.y());
+ setValue(RTIMULIB_COMPASSCAL_MAXZ, m_compassCalMax.z());
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+
+ setBlank();
+ setComment("Compass adjustment settings");
+ setComment("Compass declination is in radians and is subtracted from calculated heading");
+ setValue(RTIMULIB_COMPASSADJ_DECLINATION, m_compassAdjDeclination);
+
+ // Compass ellipsoid calibration settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+
+ setBlank();
+ setComment("Compass ellipsoid calibration");
+ setValue(RTIMULIB_COMPASSCAL_ELLIPSOID_VALID, m_compassCalEllipsoidValid);
+ setValue(RTIMULIB_COMPASSCAL_OFFSET_X, m_compassCalEllipsoidOffset.x());
+ setValue(RTIMULIB_COMPASSCAL_OFFSET_Y, m_compassCalEllipsoidOffset.y());
+ setValue(RTIMULIB_COMPASSCAL_OFFSET_Z, m_compassCalEllipsoidOffset.z());
+ setValue(RTIMULIB_COMPASSCAL_CORR11, m_compassCalEllipsoidCorr[0][0]);
+ setValue(RTIMULIB_COMPASSCAL_CORR12, m_compassCalEllipsoidCorr[0][1]);
+ setValue(RTIMULIB_COMPASSCAL_CORR13, m_compassCalEllipsoidCorr[0][2]);
+ setValue(RTIMULIB_COMPASSCAL_CORR21, m_compassCalEllipsoidCorr[1][0]);
+ setValue(RTIMULIB_COMPASSCAL_CORR22, m_compassCalEllipsoidCorr[1][1]);
+ setValue(RTIMULIB_COMPASSCAL_CORR23, m_compassCalEllipsoidCorr[1][2]);
+ setValue(RTIMULIB_COMPASSCAL_CORR31, m_compassCalEllipsoidCorr[2][0]);
+ setValue(RTIMULIB_COMPASSCAL_CORR32, m_compassCalEllipsoidCorr[2][1]);
+ setValue(RTIMULIB_COMPASSCAL_CORR33, m_compassCalEllipsoidCorr[2][2]);
+
+ // Accel calibration settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+
+ setBlank();
+ setComment("Accel calibration");
+ setValue(RTIMULIB_ACCELCAL_VALID, m_accelCalValid);
+ setValue(RTIMULIB_ACCELCAL_MINX, m_accelCalMin.x());
+ setValue(RTIMULIB_ACCELCAL_MINY, m_accelCalMin.y());
+ setValue(RTIMULIB_ACCELCAL_MINZ, m_accelCalMin.z());
+ setValue(RTIMULIB_ACCELCAL_MAXX, m_accelCalMax.x());
+ setValue(RTIMULIB_ACCELCAL_MAXY, m_accelCalMax.y());
+ setValue(RTIMULIB_ACCELCAL_MAXZ, m_accelCalMax.z());
+
+ // Gyro bias settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+
+ setBlank();
+ setComment("Saved gyro bias data");
+ setValue(RTIMULIB_GYRO_BIAS_VALID, m_gyroBiasValid);
+ setValue(RTIMULIB_GYRO_BIAS_X, m_gyroBias.x());
+ setValue(RTIMULIB_GYRO_BIAS_Y, m_gyroBias.y());
+ setValue(RTIMULIB_GYRO_BIAS_Z, m_gyroBias.z());
+
+ // MPU-9150 settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("MPU-9150 settings");
+ setComment("");
+
+ setBlank();
+ setComment("Gyro sample rate (between 5Hz and 1000Hz) ");
+ setValue(RTIMULIB_MPU9150_GYROACCEL_SAMPLERATE, m_MPU9150GyroAccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate (between 1Hz and 100Hz) ");
+ setValue(RTIMULIB_MPU9150_COMPASS_SAMPLERATE, m_MPU9150CompassSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro/accel low pass filter - ");
+ setComment(" 0 - gyro: 256Hz, accel: 260Hz");
+ setComment(" 1 - gyro: 188Hz, accel: 184Hz");
+ setComment(" 2 - gyro: 98Hz, accel: 98Hz");
+ setComment(" 3 - gyro: 42Hz, accel: 44Hz");
+ setComment(" 4 - gyro: 20Hz, accel: 21Hz");
+ setComment(" 5 - gyro: 10Hz, accel: 10Hz");
+ setComment(" 6 - gyro: 5Hz, accel: 5Hz");
+ setValue(RTIMULIB_MPU9150_GYROACCEL_LPF, m_MPU9150GyroAccelLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 - +/- 250 degress per second");
+ setComment(" 8 - +/- 500 degress per second");
+ setComment(" 16 - +/- 1000 degress per second");
+ setComment(" 24 - +/- 2000 degress per second");
+ setValue(RTIMULIB_MPU9150_GYRO_FSR, m_MPU9150GyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 - +/- 2g");
+ setComment(" 8 - +/- 4g");
+ setComment(" 16 - +/- 8g");
+ setComment(" 24 - +/- 16g");
+ setValue(RTIMULIB_MPU9150_ACCEL_FSR, m_MPU9150AccelFsr);
+
+ // MPU-9250 settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("MPU-9250 settings");
+ setComment("");
+
+ setBlank();
+ setComment("Gyro sample rate (between 5Hz and 1000Hz plus 8000Hz and 32000Hz) ");
+ setValue(RTIMULIB_MPU9250_GYROACCEL_SAMPLERATE, m_MPU9250GyroAccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate (between 1Hz and 100Hz) ");
+ setValue(RTIMULIB_MPU9250_COMPASS_SAMPLERATE, m_MPU9250CompassSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro low pass filter - ");
+ setComment(" 0x11 - 8800Hz, 0.64mS delay");
+ setComment(" 0x10 - 3600Hz, 0.11mS delay");
+ setComment(" 0x00 - 250Hz, 0.97mS delay");
+ setComment(" 0x01 - 184Hz, 2.9mS delay");
+ setComment(" 0x02 - 92Hz, 3.9mS delay");
+ setComment(" 0x03 - 41Hz, 5.9mS delay");
+ setComment(" 0x04 - 20Hz, 9.9mS delay");
+ setComment(" 0x05 - 10Hz, 17.85mS delay");
+ setComment(" 0x06 - 5Hz, 33.48mS delay");
+ setValue(RTIMULIB_MPU9250_GYRO_LPF, m_MPU9250GyroLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Accel low pass filter - ");
+ setComment(" 0x08 - 1130Hz, 0.75mS delay");
+ setComment(" 0x00 - 460Hz, 1.94mS delay");
+ setComment(" 0x01 - 184Hz, 5.80mS delay");
+ setComment(" 0x02 - 92Hz, 7.80mS delay");
+ setComment(" 0x03 - 41Hz, 11.80mS delay");
+ setComment(" 0x04 - 20Hz, 19.80mS delay");
+ setComment(" 0x05 - 10Hz, 35.70mS delay");
+ setComment(" 0x06 - 5Hz, 66.96mS delay");
+ setValue(RTIMULIB_MPU9250_ACCEL_LPF, m_MPU9250AccelLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 - +/- 250 degress per second");
+ setComment(" 8 - +/- 500 degress per second");
+ setComment(" 16 - +/- 1000 degress per second");
+ setComment(" 24 - +/- 2000 degress per second");
+ setValue(RTIMULIB_MPU9250_GYRO_FSR, m_MPU9250GyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 - +/- 2g");
+ setComment(" 8 - +/- 4g");
+ setComment(" 16 - +/- 8g");
+ setComment(" 24 - +/- 16g");
+ setValue(RTIMULIB_MPU9250_ACCEL_FSR, m_MPU9250AccelFsr);
+
+ // GD20HM303D settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("L3GD20H + LSM303D settings");
+
+ setBlank();
+ setComment("");
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 12.5Hz ");
+ setComment(" 1 = 25Hz ");
+ setComment(" 2 = 50Hz ");
+ setComment(" 3 = 100Hz ");
+ setComment(" 4 = 200Hz ");
+ setComment(" 5 = 400Hz ");
+ setComment(" 6 = 800Hz ");
+ setValue(RTIMULIB_GD20HM303D_GYRO_SAMPLERATE, m_GD20HM303DGyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 245 degrees per second ");
+ setComment(" 1 = 500 degrees per second ");
+ setComment(" 2 = 2000 degrees per second ");
+ setValue(RTIMULIB_GD20HM303D_GYRO_FSR, m_GD20HM303DGyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro high pass filter - ");
+ setComment(" 0 - 9 but see the L3GD20H manual for details");
+ setValue(RTIMULIB_GD20HM303D_GYRO_HPF, m_GD20HM303DGyroHpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro bandwidth - ");
+ setComment(" 0 - 3 but see the L3GD20H manual for details");
+ setValue(RTIMULIB_GD20HM303D_GYRO_BW, m_GD20HM303DGyroBW);
+
+ setBlank();
+ setComment("Accel sample rate - ");
+ setComment(" 1 = 3.125Hz ");
+ setComment(" 2 = 6.25Hz ");
+ setComment(" 3 = 12.5Hz ");
+ setComment(" 4 = 25Hz ");
+ setComment(" 5 = 50Hz ");
+ setComment(" 6 = 100Hz ");
+ setComment(" 7 = 200Hz ");
+ setComment(" 8 = 400Hz ");
+ setComment(" 9 = 800Hz ");
+ setComment(" 10 = 1600Hz ");
+ setValue(RTIMULIB_GD20HM303D_ACCEL_SAMPLERATE, m_GD20HM303DAccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g ");
+ setComment(" 1 = +/- 4g ");
+ setComment(" 2 = +/- 6g ");
+ setComment(" 3 = +/- 8g ");
+ setComment(" 4 = +/- 16g ");
+ setValue(RTIMULIB_GD20HM303D_ACCEL_FSR, m_GD20HM303DAccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel low pass filter - ");
+ setComment(" 0 = 773Hz");
+ setComment(" 1 = 194Hz");
+ setComment(" 2 = 362Hz");
+ setComment(" 3 = 50Hz");
+ setValue(RTIMULIB_GD20HM303D_ACCEL_LPF, m_GD20HM303DAccelLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate - ");
+ setComment(" 0 = 3.125Hz ");
+ setComment(" 1 = 6.25Hz ");
+ setComment(" 2 = 12.5Hz ");
+ setComment(" 3 = 25Hz ");
+ setComment(" 4 = 50Hz ");
+ setComment(" 5 = 100Hz ");
+ setValue(RTIMULIB_GD20HM303D_COMPASS_SAMPLERATE, m_GD20HM303DCompassSampleRate);
+
+
+ setBlank();
+ setComment("");
+ setComment("Compass full scale range - ");
+ setComment(" 0 = +/- 200 uT ");
+ setComment(" 1 = +/- 400 uT ");
+ setComment(" 2 = +/- 800 uT ");
+ setComment(" 3 = +/- 1200 uT ");
+ setValue(RTIMULIB_GD20HM303D_COMPASS_FSR, m_GD20HM303DCompassFsr);
+
+ // GD20M303DLHC settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("L3GD20 + LSM303DLHC settings");
+ setComment("");
+
+ setBlank();
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 95z ");
+ setComment(" 1 = 190Hz ");
+ setComment(" 2 = 380Hz ");
+ setComment(" 3 = 760Hz ");
+ setValue(RTIMULIB_GD20M303DLHC_GYRO_SAMPLERATE, m_GD20M303DLHCGyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 250 degrees per second ");
+ setComment(" 1 = 500 degrees per second ");
+ setComment(" 2 = 2000 degrees per second ");
+ setValue(RTIMULIB_GD20M303DLHC_GYRO_FSR, m_GD20M303DLHCGyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro high pass filter - ");
+ setComment(" 0 - 9 but see the L3GD20 manual for details");
+ setValue(RTIMULIB_GD20M303DLHC_GYRO_HPF, m_GD20M303DLHCGyroHpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro bandwidth - ");
+ setComment(" 0 - 3 but see the L3GD20 manual for details");
+ setValue(RTIMULIB_GD20M303DLHC_GYRO_BW, m_GD20M303DLHCGyroBW);
+
+ setBlank();
+ setComment("Accel sample rate - ");
+ setComment(" 1 = 1Hz ");
+ setComment(" 2 = 10Hz ");
+ setComment(" 3 = 25Hz ");
+ setComment(" 4 = 50Hz ");
+ setComment(" 5 = 100Hz ");
+ setComment(" 6 = 200Hz ");
+ setComment(" 7 = 400Hz ");
+ setValue(RTIMULIB_GD20M303DLHC_ACCEL_SAMPLERATE, m_GD20M303DLHCAccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g ");
+ setComment(" 1 = +/- 4g ");
+ setComment(" 2 = +/- 8g ");
+ setComment(" 3 = +/- 16g ");
+ setValue(RTIMULIB_GD20M303DLHC_ACCEL_FSR, m_GD20M303DLHCAccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate - ");
+ setComment(" 0 = 0.75Hz ");
+ setComment(" 1 = 1.5Hz ");
+ setComment(" 2 = 3Hz ");
+ setComment(" 3 = 7.5Hz ");
+ setComment(" 4 = 15Hz ");
+ setComment(" 5 = 30Hz ");
+ setComment(" 6 = 75Hz ");
+ setComment(" 7 = 220Hz ");
+ setValue(RTIMULIB_GD20M303DLHC_COMPASS_SAMPLERATE, m_GD20M303DLHCCompassSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Compass full scale range - ");
+ setComment(" 1 = +/- 130 uT ");
+ setComment(" 2 = +/- 190 uT ");
+ setComment(" 3 = +/- 250 uT ");
+ setComment(" 4 = +/- 400 uT ");
+ setComment(" 5 = +/- 470 uT ");
+ setComment(" 6 = +/- 560 uT ");
+ setComment(" 7 = +/- 810 uT ");
+ setValue(RTIMULIB_GD20M303DLHC_COMPASS_FSR, m_GD20M303DLHCCompassFsr);
+
+ // GD20HM303DLHC settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("L3GD20H + LSM303DLHC settings");
+ setComment("");
+
+ setBlank();
+ setComment("");
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 12.5Hz ");
+ setComment(" 1 = 25Hz ");
+ setComment(" 2 = 50Hz ");
+ setComment(" 3 = 100Hz ");
+ setComment(" 4 = 200Hz ");
+ setComment(" 5 = 400Hz ");
+ setComment(" 6 = 800Hz ");
+ setValue(RTIMULIB_GD20HM303DLHC_GYRO_SAMPLERATE, m_GD20HM303DLHCGyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 245 degrees per second ");
+ setComment(" 1 = 500 degrees per second ");
+ setComment(" 2 = 2000 degrees per second ");
+ setValue(RTIMULIB_GD20HM303DLHC_GYRO_FSR, m_GD20HM303DLHCGyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro high pass filter - ");
+ setComment(" 0 - 9 but see the L3GD20H manual for details");
+ setValue(RTIMULIB_GD20HM303DLHC_GYRO_HPF, m_GD20HM303DLHCGyroHpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro bandwidth - ");
+ setComment(" 0 - 3 but see the L3GD20H manual for details");
+ setValue(RTIMULIB_GD20HM303DLHC_GYRO_BW, m_GD20HM303DLHCGyroBW);
+ setBlank();
+ setComment("Accel sample rate - ");
+ setComment(" 1 = 1Hz ");
+ setComment(" 2 = 10Hz ");
+ setComment(" 3 = 25Hz ");
+ setComment(" 4 = 50Hz ");
+ setComment(" 5 = 100Hz ");
+ setComment(" 6 = 200Hz ");
+ setComment(" 7 = 400Hz ");
+ setValue(RTIMULIB_GD20HM303DLHC_ACCEL_SAMPLERATE, m_GD20HM303DLHCAccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g ");
+ setComment(" 1 = +/- 4g ");
+ setComment(" 2 = +/- 8g ");
+ setComment(" 3 = +/- 16g ");
+ setValue(RTIMULIB_GD20HM303DLHC_ACCEL_FSR, m_GD20HM303DLHCAccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate - ");
+ setComment(" 0 = 0.75Hz ");
+ setComment(" 1 = 1.5Hz ");
+ setComment(" 2 = 3Hz ");
+ setComment(" 3 = 7.5Hz ");
+ setComment(" 4 = 15Hz ");
+ setComment(" 5 = 30Hz ");
+ setComment(" 6 = 75Hz ");
+ setComment(" 7 = 220Hz ");
+ setValue(RTIMULIB_GD20HM303DLHC_COMPASS_SAMPLERATE, m_GD20HM303DLHCCompassSampleRate);
+
+
+ setBlank();
+ setComment("");
+ setComment("Compass full scale range - ");
+ setComment(" 1 = +/- 130 uT ");
+ setComment(" 2 = +/- 190 uT ");
+ setComment(" 3 = +/- 250 uT ");
+ setComment(" 4 = +/- 400 uT ");
+ setComment(" 5 = +/- 470 uT ");
+ setComment(" 6 = +/- 560 uT ");
+ setComment(" 7 = +/- 810 uT ");
+ setValue(RTIMULIB_GD20HM303DLHC_COMPASS_FSR, m_GD20HM303DLHCCompassFsr);
+
+ // LSM9DS0 settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("LSM9DS0 settings");
+ setComment("");
+
+ setBlank();
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 95z ");
+ setComment(" 1 = 190Hz ");
+ setComment(" 2 = 380Hz ");
+ setComment(" 3 = 760Hz ");
+ setValue(RTIMULIB_LSM9DS0_GYRO_SAMPLERATE, m_LSM9DS0GyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 250 degrees per second ");
+ setComment(" 1 = 500 degrees per second ");
+ setComment(" 2 = 2000 degrees per second ");
+ setValue(RTIMULIB_LSM9DS0_GYRO_FSR, m_LSM9DS0GyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro high pass filter - ");
+ setComment(" 0 - 9 but see the LSM9DS0 manual for details");
+ setValue(RTIMULIB_LSM9DS0_GYRO_HPF, m_LSM9DS0GyroHpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro bandwidth - ");
+ setComment(" 0 - 3 but see the LSM9DS0 manual for details");
+ setValue(RTIMULIB_LSM9DS0_GYRO_BW, m_LSM9DS0GyroBW);
+
+ setBlank();
+ setComment("Accel sample rate - ");
+ setComment(" 1 = 3.125Hz ");
+ setComment(" 2 = 6.25Hz ");
+ setComment(" 3 = 12.5Hz ");
+ setComment(" 4 = 25Hz ");
+ setComment(" 5 = 50Hz ");
+ setComment(" 6 = 100Hz ");
+ setComment(" 7 = 200Hz ");
+ setComment(" 8 = 400Hz ");
+ setComment(" 9 = 800Hz ");
+ setComment(" 10 = 1600Hz ");
+ setValue(RTIMULIB_LSM9DS0_ACCEL_SAMPLERATE, m_LSM9DS0AccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g ");
+ setComment(" 1 = +/- 4g ");
+ setComment(" 2 = +/- 6g ");
+ setComment(" 3 = +/- 8g ");
+ setComment(" 4 = +/- 16g ");
+ setValue(RTIMULIB_LSM9DS0_ACCEL_FSR, m_LSM9DS0AccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel low pass filter - ");
+ setComment(" 0 = 773Hz");
+ setComment(" 1 = 194Hz");
+ setComment(" 2 = 362Hz");
+ setComment(" 3 = 50Hz");
+ setValue(RTIMULIB_LSM9DS0_ACCEL_LPF, m_LSM9DS0AccelLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate - ");
+ setComment(" 0 = 3.125Hz ");
+ setComment(" 1 = 6.25Hz ");
+ setComment(" 2 = 12.5Hz ");
+ setComment(" 3 = 25Hz ");
+ setComment(" 4 = 50Hz ");
+ setComment(" 5 = 100Hz ");
+ setValue(RTIMULIB_LSM9DS0_COMPASS_SAMPLERATE, m_LSM9DS0CompassSampleRate);
+
+
+ setBlank();
+ setComment("");
+ setComment("Compass full scale range - ");
+ setComment(" 0 = +/- 200 uT ");
+ setComment(" 1 = +/- 400 uT ");
+ setComment(" 2 = +/- 800 uT ");
+ setComment(" 3 = +/- 1200 uT ");
+ setValue(RTIMULIB_LSM9DS0_COMPASS_FSR, m_LSM9DS0CompassFsr);
+
+// LSM9DS1 settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("LSM9DS1 settings");
+ setComment("");
+
+ setBlank();
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 95Hz ");
+ setComment(" 1 = 190Hz ");
+ setComment(" 2 = 380Hz ");
+ setComment(" 3 = 760Hz ");
+ setValue(RTIMULIB_LSM9DS1_GYRO_SAMPLERATE, m_LSM9DS1GyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 250 degrees per second ");
+ setComment(" 1 = 500 degrees per second ");
+ setComment(" 2 = 2000 degrees per second ");
+ setValue(RTIMULIB_LSM9DS1_GYRO_FSR, m_LSM9DS1GyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro high pass filter - ");
+ setComment(" 0 - 9 but see the LSM9DS1 manual for details");
+ setValue(RTIMULIB_LSM9DS1_GYRO_HPF, m_LSM9DS1GyroHpf);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro bandwidth - ");
+ setComment(" 0 - 3 but see the LSM9DS1 manual for details");
+ setValue(RTIMULIB_LSM9DS1_GYRO_BW, m_LSM9DS1GyroBW);
+
+ setBlank();
+ setComment("Accel sample rate - ");
+ setComment(" 1 = 14.9Hz ");
+ setComment(" 2 = 59.5Hz ");
+ setComment(" 3 = 119Hz ");
+ setComment(" 4 = 238Hz ");
+ setComment(" 5 = 476Hz ");
+ setComment(" 6 = 952Hz ");
+ setValue(RTIMULIB_LSM9DS1_ACCEL_SAMPLERATE, m_LSM9DS1AccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g ");
+ setComment(" 1 = +/- 16g ");
+ setComment(" 2 = +/- 4g ");
+ setComment(" 3 = +/- 8g ");
+ setValue(RTIMULIB_LSM9DS1_ACCEL_FSR, m_LSM9DS1AccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel low pass filter - ");
+ setComment(" 0 = 408Hz");
+ setComment(" 1 = 211Hz");
+ setComment(" 2 = 105Hz");
+ setComment(" 3 = 50Hz");
+ setValue(RTIMULIB_LSM9DS1_ACCEL_LPF, m_LSM9DS1AccelLpf);
+
+ setBlank();
+ setComment("");
+ setComment("Compass sample rate - ");
+ setComment(" 0 = 0.625Hz ");
+ setComment(" 1 = 1.25Hz ");
+ setComment(" 2 = 2.5Hz ");
+ setComment(" 3 = 5Hz ");
+ setComment(" 4 = 10Hz ");
+ setComment(" 5 = 20Hz ");
+ setComment(" 6 = 40Hz ");
+ setComment(" 7 = 80Hz ");
+ setValue(RTIMULIB_LSM9DS1_COMPASS_SAMPLERATE, m_LSM9DS1CompassSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Compass full scale range - ");
+ setComment(" 0 = +/- 400 uT ");
+ setComment(" 1 = +/- 800 uT ");
+ setComment(" 2 = +/- 1200 uT ");
+ setComment(" 3 = +/- 1600 uT ");
+ setValue(RTIMULIB_LSM9DS1_COMPASS_FSR, m_LSM9DS1CompassFsr);
+
+ // BMX055 settings
+
+ setBlank();
+ setComment("#####################################################################");
+ setComment("");
+ setComment("BMX055 settings");
+ setComment("");
+
+ setBlank();
+ setComment("");
+ setComment("Gyro sample rate - ");
+ setComment(" 0 = 2000Hz (532Hz filter)");
+ setComment(" 1 = 2000Hz (230Hz filter)");
+ setComment(" 2 = 1000Hz (116Hz filter)");
+ setComment(" 3 = 400Hz (47Hz filter)");
+ setComment(" 4 = 200Hz (23Hz filter)");
+ setComment(" 5 = 100Hz (12Hz filter)");
+ setComment(" 6 = 200Hz (64Hz filter)");
+ setComment(" 7 = 100Hz (32Hz filter)");
+ setValue(RTIMULIB_BMX055_GYRO_SAMPLERATE, m_BMX055GyroSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Gyro full scale range - ");
+ setComment(" 0 = 2000 deg/s");
+ setComment(" 1 = 1000 deg/s");
+ setComment(" 2 = 500 deg/s");
+ setComment(" 3 = 250 deg/s");
+ setComment(" 4 = 125 deg/s");
+ setValue(RTIMULIB_BMX055_GYRO_FSR, m_BMX055GyroFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Accel sample rate - ");
+ setComment(" 0 = 15.63Hz");
+ setComment(" 1 = 31.25");
+ setComment(" 2 = 62.5");
+ setComment(" 3 = 125");
+ setComment(" 4 = 250");
+ setComment(" 5 = 500");
+ setComment(" 6 = 1000");
+ setComment(" 7 = 2000");
+ setValue(RTIMULIB_BMX055_ACCEL_SAMPLERATE, m_BMX055AccelSampleRate);
+
+ setBlank();
+ setComment("");
+ setComment("Accel full scale range - ");
+ setComment(" 0 = +/- 2g");
+ setComment(" 1 = +/- 4g");
+ setComment(" 2 = +/- 8g");
+ setComment(" 3 = +/- 16g");
+ setValue(RTIMULIB_BMX055_ACCEL_FSR, m_BMX055AccelFsr);
+
+ setBlank();
+ setComment("");
+ setComment("Mag presets - ");
+ setComment(" 0 = Low power");
+ setComment(" 1 = Regular");
+ setComment(" 2 = Enhanced");
+ setComment(" 3 = High accuracy");
+ setValue(RTIMULIB_BMX055_MAG_PRESET, m_BMX055MagPreset);
+
+ fclose(m_fd);
+ return true;
+}
+
+void RTIMUSettings::setBlank()
+{
+ fprintf(m_fd, "\n");
+}
+
+void RTIMUSettings::setComment(const char *comment)
+{
+ fprintf(m_fd, "# %s\n", comment);
+}
+
+void RTIMUSettings::setValue(const char *key, const bool val)
+{
+ fprintf(m_fd, "%s=%s\n", key, val ? "true" : "false");
+}
+
+void RTIMUSettings::setValue(const char *key, const int val)
+{
+ fprintf(m_fd, "%s=%d\n", key, val);
+}
+
+void RTIMUSettings::setValue(const char *key, const RTFLOAT val)
+{
+ fprintf(m_fd, "%s=%f\n", key, val);
+}
+
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.h b/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.h
new file mode 100644
index 0000000..14bae57
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTIMUSettings.h
@@ -0,0 +1,367 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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
+// staslock@gmail.com (www.clickdrive.io)
+
+#ifndef _RTIMUSETTINGS_H
+#define _RTIMUSETTINGS_H
+
+#include "RTMath.h"
+#include "RTIMUHal.h"
+
+// Settings keys
+
+#define RTIMULIB_IMU_TYPE "IMUType"
+#define RTIMULIB_FUSION_TYPE "FusionType"
+#define RTIMULIB_BUS_IS_I2C "BusIsI2C"
+#define RTIMULIB_I2C_SLAVEADDRESS "I2CSlaveAddress"
+#define RTIMULIB_I2C_BUS "I2CBus"
+#define RTIMULIB_SPI_BUS "SPIBus"
+#define RTIMULIB_SPI_SELECT "SPISelect"
+#define RTIMULIB_SPI_SPEED "SPISpeed"
+#define RTIMULIB_AXIS_ROTATION "AxisRotation"
+#define RTIMULIB_PRESSURE_TYPE "PressureType"
+#define RTIMULIB_I2C_PRESSUREADDRESS "I2CPressureAddress"
+#define RTIMULIB_HUMIDITY_TYPE "HumidityType"
+#define RTIMULIB_I2C_HUMIDITYADDRESS "I2CHumidityAddress"
+
+// MPU9150 settings keys
+
+#define RTIMULIB_MPU9150_GYROACCEL_SAMPLERATE "MPU9150GyroAccelSampleRate"
+#define RTIMULIB_MPU9150_COMPASS_SAMPLERATE "MPU9150CompassSampleRate"
+#define RTIMULIB_MPU9150_GYROACCEL_LPF "MPU9150GyroAccelLpf"
+#define RTIMULIB_MPU9150_GYRO_FSR "MPU9150GyroFSR"
+#define RTIMULIB_MPU9150_ACCEL_FSR "MPU9150AccelFSR"
+
+// MPU9250 settings keys
+
+#define RTIMULIB_MPU9250_GYROACCEL_SAMPLERATE "MPU9250GyroAccelSampleRate"
+#define RTIMULIB_MPU9250_COMPASS_SAMPLERATE "MPU9250CompassSampleRate"
+#define RTIMULIB_MPU9250_GYRO_LPF "MPU9250GyroLpf"
+#define RTIMULIB_MPU9250_ACCEL_LPF "MPU9250AccelLpf"
+#define RTIMULIB_MPU9250_GYRO_FSR "MPU9250GyroFSR"
+#define RTIMULIB_MPU9250_ACCEL_FSR "MPU9250AccelFSR"
+
+// GD20HM303D settings keys
+
+#define RTIMULIB_GD20HM303D_GYRO_SAMPLERATE "GD20HM303DGyroSampleRate"
+#define RTIMULIB_GD20HM303D_GYRO_BW "GD20HM303DGyroBW"
+#define RTIMULIB_GD20HM303D_GYRO_HPF "GD20HM303DGyroHpf"
+#define RTIMULIB_GD20HM303D_GYRO_FSR "GD20HM303DGyroFsr"
+
+#define RTIMULIB_GD20HM303D_ACCEL_SAMPLERATE "GD20HM303DAccelSampleRate"
+#define RTIMULIB_GD20HM303D_ACCEL_FSR "GD20HM303DAccelFsr"
+#define RTIMULIB_GD20HM303D_ACCEL_LPF "GD20HM303DAccelLpf"
+
+#define RTIMULIB_GD20HM303D_COMPASS_SAMPLERATE "GD20HM303DCompassSampleRate"
+#define RTIMULIB_GD20HM303D_COMPASS_FSR "GD20HM303DCompassFsr"
+
+
+// GD20M303DLHC settings keys
+
+#define RTIMULIB_GD20M303DLHC_GYRO_SAMPLERATE "GD20M303DLHCGyroSampleRate"
+#define RTIMULIB_GD20M303DLHC_GYRO_BW "GD20M303DLHCGyroBW"
+#define RTIMULIB_GD20M303DLHC_GYRO_HPF "GD20M303DLHCGyroHpf"
+#define RTIMULIB_GD20M303DLHC_GYRO_FSR "GD20M303DLHCGyroFsr"
+
+#define RTIMULIB_GD20M303DLHC_ACCEL_SAMPLERATE "GD20M303DLHCAccelSampleRate"
+#define RTIMULIB_GD20M303DLHC_ACCEL_FSR "GD20M303DLHCAccelFsr"
+
+#define RTIMULIB_GD20M303DLHC_COMPASS_SAMPLERATE "GD20M303DLHCCompassSampleRate"
+#define RTIMULIB_GD20M303DLHC_COMPASS_FSR "GD20M303DLHCCompassFsr"
+
+// GD20HM303DLHC settings keys
+
+#define RTIMULIB_GD20HM303DLHC_GYRO_SAMPLERATE "GD20HM303DLHCGyroSampleRate"
+#define RTIMULIB_GD20HM303DLHC_GYRO_BW "GD20HM303DLHCGyroBW"
+#define RTIMULIB_GD20HM303DLHC_GYRO_HPF "GD20HM303DLHCGyroHpf"
+#define RTIMULIB_GD20HM303DLHC_GYRO_FSR "GD20HM303DLHCGyroFsr"
+
+#define RTIMULIB_GD20HM303DLHC_ACCEL_SAMPLERATE "GD20HM303DLHCAccelSampleRate"
+#define RTIMULIB_GD20HM303DLHC_ACCEL_FSR "GD20HM303DLHCAccelFsr"
+
+#define RTIMULIB_GD20HM303DLHC_COMPASS_SAMPLERATE "GD20HM303DLHCCompassSampleRate"
+#define RTIMULIB_GD20HM303DLHC_COMPASS_FSR "GD20HM303DLHCCompassFsr"
+
+
+// LSM9DS0 settings keys
+
+#define RTIMULIB_LSM9DS0_GYRO_SAMPLERATE "LSM9DS0GyroSampleRate"
+#define RTIMULIB_LSM9DS0_GYRO_BW "LSM9DS0GyroBW"
+#define RTIMULIB_LSM9DS0_GYRO_HPF "LSM9DS0GyroHpf"
+#define RTIMULIB_LSM9DS0_GYRO_FSR "LSM9DS0GyroFsr"
+
+#define RTIMULIB_LSM9DS0_ACCEL_SAMPLERATE "LSM9DS0AccelSampleRate"
+#define RTIMULIB_LSM9DS0_ACCEL_FSR "LSM9DS0AccelFsr"
+#define RTIMULIB_LSM9DS0_ACCEL_LPF "LSM9DS0AccelLpf"
+
+#define RTIMULIB_LSM9DS0_COMPASS_SAMPLERATE "LSM9DS0CompassSampleRate"
+#define RTIMULIB_LSM9DS0_COMPASS_FSR "LSM9DS0CompassFsr"
+
+// LSM9DS1 settings keys
+
+#define RTIMULIB_LSM9DS1_GYRO_SAMPLERATE "LSM9DS1GyroSampleRate"
+#define RTIMULIB_LSM9DS1_GYRO_BW "LSM9DS1GyroBW"
+#define RTIMULIB_LSM9DS1_GYRO_HPF "LSM9DS1GyroHpf"
+#define RTIMULIB_LSM9DS1_GYRO_FSR "LSM9DS1GyroFsr"
+
+#define RTIMULIB_LSM9DS1_ACCEL_SAMPLERATE "LSM9DS1AccelSampleRate"
+#define RTIMULIB_LSM9DS1_ACCEL_FSR "LSM9DS1AccelFsr"
+#define RTIMULIB_LSM9DS1_ACCEL_LPF "LSM9DS1AccelLpf"
+
+#define RTIMULIB_LSM9DS1_COMPASS_SAMPLERATE "LSM9DS1CompassSampleRate"
+#define RTIMULIB_LSM9DS1_COMPASS_FSR "LSM9DS1CompassFsr"
+
+// BMX055 settings keys
+
+#define RTIMULIB_BMX055_GYRO_SAMPLERATE "BMX055GyroSampleRate"
+#define RTIMULIB_BMX055_GYRO_FSR "BMX055GyroFsr"
+
+#define RTIMULIB_BMX055_ACCEL_SAMPLERATE "BMX055AccelSampleRate"
+#define RTIMULIB_BMX055_ACCEL_FSR "BMX055AccelFsr"
+
+#define RTIMULIB_BMX055_MAG_PRESET "BMX055MagPreset"
+
+// Gyro bias keys
+
+#define RTIMULIB_GYRO_BIAS_VALID "GyroBiasValid"
+#define RTIMULIB_GYRO_BIAS_X "GyroBiasX"
+#define RTIMULIB_GYRO_BIAS_Y "GyroBiasY"
+#define RTIMULIB_GYRO_BIAS_Z "GyroBiasZ"
+
+// Compass calibration and adjustment settings keys
+
+#define RTIMULIB_COMPASSCAL_VALID "CompassCalValid"
+#define RTIMULIB_COMPASSCAL_MINX "CompassCalMinX"
+#define RTIMULIB_COMPASSCAL_MAXX "CompassCalMaxX"
+#define RTIMULIB_COMPASSCAL_MINY "CompassCalMinY"
+#define RTIMULIB_COMPASSCAL_MAXY "CompassCalMaxY"
+#define RTIMULIB_COMPASSCAL_MINZ "CompassCalMinZ"
+#define RTIMULIB_COMPASSCAL_MAXZ "CompassCalMaxZ"
+
+#define RTIMULIB_COMPASSCAL_ELLIPSOID_VALID "compassCalEllipsoidValid"
+#define RTIMULIB_COMPASSCAL_OFFSET_X "compassCalOffsetX"
+#define RTIMULIB_COMPASSCAL_OFFSET_Y "compassCalOffsetY"
+#define RTIMULIB_COMPASSCAL_OFFSET_Z "compassCalOffsetZ"
+#define RTIMULIB_COMPASSCAL_CORR11 "compassCalCorr11"
+#define RTIMULIB_COMPASSCAL_CORR12 "compassCalCorr12"
+#define RTIMULIB_COMPASSCAL_CORR13 "compassCalCorr13"
+#define RTIMULIB_COMPASSCAL_CORR21 "compassCalCorr21"
+#define RTIMULIB_COMPASSCAL_CORR22 "compassCalCorr22"
+#define RTIMULIB_COMPASSCAL_CORR23 "compassCalCorr23"
+#define RTIMULIB_COMPASSCAL_CORR31 "compassCalCorr31"
+#define RTIMULIB_COMPASSCAL_CORR32 "compassCalCorr32"
+#define RTIMULIB_COMPASSCAL_CORR33 "compassCalCorr33"
+
+#define RTIMULIB_COMPASSADJ_DECLINATION "compassAdjDeclination"
+
+// Accel calibration settings keys
+
+#define RTIMULIB_ACCELCAL_VALID "AccelCalValid"
+#define RTIMULIB_ACCELCAL_MINX "AccelCalMinX"
+#define RTIMULIB_ACCELCAL_MAXX "AccelCalMaxX"
+#define RTIMULIB_ACCELCAL_MINY "AccelCalMinY"
+#define RTIMULIB_ACCELCAL_MAXY "AccelCalMaxY"
+#define RTIMULIB_ACCELCAL_MINZ "AccelCalMinZ"
+#define RTIMULIB_ACCELCAL_MAXZ "AccelCalMaxZ"
+
+
+class RTIMUSettings : public RTIMUHal
+{
+public:
+
+ // Standard constructor sets up for ini file in working directory
+
+ RTIMUSettings(const char *productType = "RTIMULib");
+
+ // Alternate constructor allow ini file to be in any directory
+
+ RTIMUSettings(const char *settingsDirectory, const char *productType);
+
+ // This function tries to find an IMU. It stops at the first valid one
+ // and returns true or else false
+
+ bool discoverIMU(int& imuType, bool& busIsI2C, unsigned char& slaveAddress);
+
+ // This function tries to find a pressure sensor. It stops at the first valid one
+ // and returns true or else false
+
+ bool discoverPressure(int& pressureType, unsigned char& pressureAddress);
+
+ // This function tries to find a humidity sensor. It stops at the first valid one
+ // and returns true or else false
+
+ bool discoverHumidity(int& humidityType, unsigned char& humidityAddress);
+
+ // This function sets the settings to default values.
+
+ void setDefaults();
+
+ // This function loads the local variables from the settings file or uses defaults
+
+ virtual bool loadSettings();
+
+ // This function saves the local variables to the settings file
+
+ virtual bool saveSettings();
+
+ // These are the local variables
+
+ int m_imuType; // type code of imu in use
+ int m_fusionType; // fusion algorithm type code
+ unsigned char m_I2CSlaveAddress; // I2C slave address of the imu
+ int m_axisRotation; // axis rotation code
+ int m_pressureType; // type code of pressure sensor in use
+ unsigned char m_I2CPressureAddress; // I2C slave address of the pressure sensor
+ int m_humidityType; // type code of humidity sensor in use
+ unsigned char m_I2CHumidityAddress; // I2C slave address of the humidity sensor
+
+ bool m_compassCalValid; // true if there is valid compass calibration data
+ RTVector3 m_compassCalMin; // the minimum values
+ RTVector3 m_compassCalMax; // the maximum values
+
+ bool m_compassCalEllipsoidValid; // true if the ellipsoid calibration data is valid
+ RTVector3 m_compassCalEllipsoidOffset; // the ellipsoid offset
+ float m_compassCalEllipsoidCorr[3][3]; // the correction matrix
+
+ float m_compassAdjDeclination; // magnetic declination adjustment - subtracted from measured
+
+ bool m_accelCalValid; // true if there is valid accel calibration data
+ RTVector3 m_accelCalMin; // the minimum values
+ RTVector3 m_accelCalMax; // the maximum values
+
+ bool m_gyroBiasValid; // true if the recorded gyro bias is valid
+ RTVector3 m_gyroBias; // the recorded gyro bias
+
+ // IMU-specific vars
+
+ // MPU9150
+
+ int m_MPU9150GyroAccelSampleRate; // the sample rate (samples per second) for gyro and accel
+ int m_MPU9150CompassSampleRate; // same for the compass
+ int m_MPU9150GyroAccelLpf; // low pass filter code for the gyro and accel
+ int m_MPU9150GyroFsr; // FSR code for the gyro
+ int m_MPU9150AccelFsr; // FSR code for the accel
+
+ // MPU9250
+
+ int m_MPU9250GyroAccelSampleRate; // the sample rate (samples per second) for gyro and accel
+ int m_MPU9250CompassSampleRate; // same for the compass
+ int m_MPU9250GyroLpf; // low pass filter code for the gyro
+ int m_MPU9250AccelLpf; // low pass filter code for the accel
+ int m_MPU9250GyroFsr; // FSR code for the gyro
+ int m_MPU9250AccelFsr; // FSR code for the accel
+
+ // GD20HM303D
+
+ int m_GD20HM303DGyroSampleRate; // the gyro sample rate
+ int m_GD20HM303DGyroBW; // the gyro bandwidth code
+ int m_GD20HM303DGyroHpf; // the gyro high pass filter cutoff code
+ int m_GD20HM303DGyroFsr; // the gyro full scale range
+
+ int m_GD20HM303DAccelSampleRate; // the accel sample rate
+ int m_GD20HM303DAccelFsr; // the accel full scale range
+ int m_GD20HM303DAccelLpf; // the accel low pass filter
+
+ int m_GD20HM303DCompassSampleRate; // the compass sample rate
+ int m_GD20HM303DCompassFsr; // the compass full scale range
+
+ // GD20M303DLHC
+
+ int m_GD20M303DLHCGyroSampleRate; // the gyro sample rate
+ int m_GD20M303DLHCGyroBW; // the gyro bandwidth code
+ int m_GD20M303DLHCGyroHpf; // the gyro high pass filter cutoff code
+ int m_GD20M303DLHCGyroFsr; // the gyro full scale range
+
+ int m_GD20M303DLHCAccelSampleRate; // the accel sample rate
+ int m_GD20M303DLHCAccelFsr; // the accel full scale range
+
+ int m_GD20M303DLHCCompassSampleRate; // the compass sample rate
+ int m_GD20M303DLHCCompassFsr; // the compass full scale range
+
+ // GD20HM303DLHC
+
+ int m_GD20HM303DLHCGyroSampleRate; // the gyro sample rate
+ int m_GD20HM303DLHCGyroBW; // the gyro bandwidth code
+ int m_GD20HM303DLHCGyroHpf; // the gyro high pass filter cutoff code
+ int m_GD20HM303DLHCGyroFsr; // the gyro full scale range
+
+ int m_GD20HM303DLHCAccelSampleRate; // the accel sample rate
+ int m_GD20HM303DLHCAccelFsr; // the accel full scale range
+
+ int m_GD20HM303DLHCCompassSampleRate; // the compass sample rate
+ int m_GD20HM303DLHCCompassFsr; // the compass full scale range
+
+ // LSM9DS0
+
+ int m_LSM9DS0GyroSampleRate; // the gyro sample rate
+ int m_LSM9DS0GyroBW; // the gyro bandwidth code
+ int m_LSM9DS0GyroHpf; // the gyro high pass filter cutoff code
+ int m_LSM9DS0GyroFsr; // the gyro full scale range
+
+ int m_LSM9DS0AccelSampleRate; // the accel sample rate
+ int m_LSM9DS0AccelFsr; // the accel full scale range
+ int m_LSM9DS0AccelLpf; // the accel low pass filter
+
+ int m_LSM9DS0CompassSampleRate; // the compass sample rate
+ int m_LSM9DS0CompassFsr; // the compass full scale range
+
+ // LSM9DS1
+
+ int m_LSM9DS1GyroSampleRate; // the gyro sample rate
+ int m_LSM9DS1GyroBW; // the gyro bandwidth code
+ int m_LSM9DS1GyroHpf; // the gyro high pass filter cutoff code
+ int m_LSM9DS1GyroFsr; // the gyro full scale range
+
+ int m_LSM9DS1AccelSampleRate; // the accel sample rate
+ int m_LSM9DS1AccelFsr; // the accel full scale range
+ int m_LSM9DS1AccelLpf; // the accel low pass filter
+
+ int m_LSM9DS1CompassSampleRate; // the compass sample rate
+ int m_LSM9DS1CompassFsr; // the compass full scale range
+
+ // BMX055
+
+ int m_BMX055GyroSampleRate; // the gyro sample rate
+ int m_BMX055GyroFsr; // the gyro full scale range
+
+ int m_BMX055AccelSampleRate; // the accel sample rate
+ int m_BMX055AccelFsr; // the accel full scale range
+
+ int m_BMX055MagPreset; // the mag preset code
+
+private:
+ void setBlank();
+ void setComment(const char *comment);
+ void setValue(const char *key, const bool val);
+ void setValue(const char *key, const int val);
+ void setValue(const char *key, const RTFLOAT val);
+
+ char m_filename[256]; // the settings file name
+
+ FILE *m_fd;
+};
+
+#endif // _RTIMUSETTINGS_H
+
[8/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
MINIFICPP-517: Add RTIMULib and create basic functionality.
Ported from other work, this package contains two processors that create
simple flow files. This approach will likely evolve to improve accessing
the sensor data from the Processors and decorating flow files.
This closes #348.
Signed-off-by: Aldrin Piri <al...@apache.org>
Project: http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/repo
Commit: http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/commit/9dbad3bd
Tree: http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/tree/9dbad3bd
Diff: http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/diff/9dbad3bd
Branch: refs/heads/master
Commit: 9dbad3bdec02431cc31dcb7e0019fff65c9f80fe
Parents: 3156c1e
Author: Marc Parisi <ph...@apache.org>
Authored: Tue May 29 21:51:29 2018 -0400
Committer: Aldrin Piri <al...@apache.org>
Committed: Wed Jun 6 11:58:03 2018 -0400
----------------------------------------------------------------------
CMakeLists.txt | 11 +-
LICENSE | 25 +
extensions/ExtensionHeader.txt | 26 +
extensions/sensors/CMakeLists.txt | 56 +
extensions/sensors/GetEnvironmentalSensors.cpp | 155 ++
extensions/sensors/GetEnvironmentalSensors.h | 85 +
extensions/sensors/GetMovementSensors.cpp | 97 +
extensions/sensors/GetMovementSensors.h | 77 +
extensions/sensors/README.MD | 20 +
extensions/sensors/SensorBase.cpp | 75 +
extensions/sensors/SensorBase.h | 92 +
extensions/sensors/SensorLoader.cpp | 30 +
extensions/sensors/SensorLoader.h | 71 +
libminifi/test/resources/TestEnvironmental.yml | 71 +
libminifi/test/sensors-tests/CMakeLists.txt | 42 +
libminifi/test/sensors-tests/SensorTests.cpp | 100 +
thirdparty/RTIMULib/.gitignore | 53 +
thirdparty/RTIMULib/LICENSE | 29 +
thirdparty/RTIMULib/README.md | 370 ++++
thirdparty/RTIMULib/RTIMULib/CMakeLists.txt | 97 +
.../RTIMULib/RTIMULib/IMUDrivers/RTHumidity.cpp | 62 +
.../RTIMULib/RTIMULib/IMUDrivers/RTHumidity.h | 55 +
.../RTIMULib/IMUDrivers/RTHumidityDefs.h | 82 +
.../RTIMULib/IMUDrivers/RTHumidityHTS221.cpp | 142 ++
.../RTIMULib/IMUDrivers/RTHumidityHTS221.h | 57 +
.../RTIMULib/IMUDrivers/RTHumidityHTU21D.cpp | 126 ++
.../RTIMULib/IMUDrivers/RTHumidityHTU21D.h | 57 +
.../RTIMULib/RTIMULib/IMUDrivers/RTIMU.cpp | 478 +++++
thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.h | 200 ++
.../RTIMULib/IMUDrivers/RTIMUBMX055.cpp | 971 ++++++++++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.h | 80 +
.../RTIMULib/IMUDrivers/RTIMUBNO055.cpp | 191 ++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h | 48 +
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h | 1119 ++++++++++++
.../RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp | 563 ++++++
.../RTIMULib/IMUDrivers/RTIMUGD20HM303D.h | 95 +
.../RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp | 562 ++++++
.../RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h | 98 +
.../RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp | 537 ++++++
.../RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h | 97 +
.../RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp | 538 ++++++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h | 95 +
.../RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp | 408 +++++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h | 93 +
.../RTIMULib/IMUDrivers/RTIMUMPU9150.cpp | 633 +++++++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h | 110 ++
.../RTIMULib/IMUDrivers/RTIMUMPU9250.cpp | 657 +++++++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h | 118 ++
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp | 54 +
.../RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h | 58 +
.../RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp | 70 +
.../RTIMULib/RTIMULib/IMUDrivers/RTPressure.h | 55 +
.../RTIMULib/IMUDrivers/RTPressureBMP180.cpp | 230 +++
.../RTIMULib/IMUDrivers/RTPressureBMP180.h | 88 +
.../RTIMULib/IMUDrivers/RTPressureDefs.h | 105 ++
.../RTIMULib/IMUDrivers/RTPressureLPS25H.cpp | 91 +
.../RTIMULib/IMUDrivers/RTPressureLPS25H.h | 53 +
.../RTIMULib/IMUDrivers/RTPressureMS5611.cpp | 170 ++
.../RTIMULib/IMUDrivers/RTPressureMS5611.h | 69 +
.../RTIMULib/IMUDrivers/RTPressureMS5637.cpp | 172 ++
.../RTIMULib/IMUDrivers/RTPressureMS5637.h | 69 +
thirdparty/RTIMULib/RTIMULib/RTFusion.cpp | 137 ++
thirdparty/RTIMULib/RTIMULib/RTFusion.h | 105 ++
.../RTIMULib/RTIMULib/RTFusionKalman4.cpp | 238 +++
thirdparty/RTIMULib/RTIMULib/RTFusionKalman4.h | 79 +
thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.cpp | 163 ++
thirdparty/RTIMULib/RTIMULib/RTFusionRTQF.h | 62 +
thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.cpp | 103 ++
thirdparty/RTIMULib/RTIMULib/RTIMUAccelCal.h | 74 +
thirdparty/RTIMULib/RTIMULib/RTIMUCalDefs.h | 57 +
thirdparty/RTIMULib/RTIMULib/RTIMUHal.cpp | 407 +++++
thirdparty/RTIMULib/RTIMULib/RTIMUHal.h | 116 ++
thirdparty/RTIMULib/RTIMULib/RTIMULIB LICENSE | 26 +
thirdparty/RTIMULib/RTIMULib/RTIMULib.h | 54 +
thirdparty/RTIMULib/RTIMULib/RTIMULib.pri | 90 +
thirdparty/RTIMULib/RTIMULib/RTIMULibDefs.h | 64 +
thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp | 260 +++
thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.h | 98 +
thirdparty/RTIMULib/RTIMULib/RTIMUSettings.cpp | 1723 ++++++++++++++++++
thirdparty/RTIMULib/RTIMULib/RTIMUSettings.h | 367 ++++
thirdparty/RTIMULib/RTIMULib/RTMath.cpp | 630 +++++++
thirdparty/RTIMULib/RTIMULib/RTMath.h | 195 ++
thirdparty/RTIMULib/RTIMULibVersion.txt | 6 +
83 files changed, 16189 insertions(+), 3 deletions(-)
----------------------------------------------------------------------
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/CMakeLists.txt
----------------------------------------------------------------------
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 287e44d..3e44507 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,7 +196,7 @@ endif()
add_subdirectory(libminifi)
-#### EXTENSION
+#### EXTENSIONS
option(DISABLE_CURL "Disables libCurl Properties." OFF)
if (NOT DISABLE_CURL AND NOT DISABLE_CIVET)
createExtension(HTTP-CURL "HTTP CURL" "This enables RESTProtocol, InvokeHTTP, and the HTTPClient for Site to Site" "extensions/http-curl" "extensions/http-curl/tests/")
@@ -259,6 +259,13 @@ if (NOT DISABLE_SCRIPTING)
createExtension(SCRIPTING-EXTENSIONS "SCRIPTING EXTENSIONS" "This enables scripting" "extensions/script" "${TEST_DIR}/script-tests")
endif()
+option(ENABLE_SENSORS "Enables the Sensors package." OFF)
+if(ENABLE_ALL OR ENABLE_SENSORS)
+ add_subdirectory(thirdparty/RTIMULib/RTIMULib)
+ createExtension(SENSOR-EXTENSIONS "SENSOR EXTENSIONS" "Enables the package of sensor extensions." "extensions/sensors" "${TEST_DIR}/sensors-tests")
+endif()
+
+
## SQLite extensions
option(ENABLE_SQLITE "Disables the scripting extensions." OFF)
if (ENABLE_ALL OR ENABLE_SQLITE)
@@ -296,9 +303,7 @@ endif()
get_property(selected_extensions GLOBAL PROPERTY EXTENSION-OPTIONS)
if (NOT BUILD_IDENTIFIER)
- message("empty identifier ${BUILD_IDENTIFIER}")
if (NOT BUILD_IDENTIFIER)
- message("empty identifier ${BUILD_IDENTIFIER}")
string(RANDOM LENGTH 24 BUILD_IDENTIFIER)
set(BUILD_IDENTIFIER "${BUILD_IDENTIFIER}" CACHE STRING "Build identifier")
endif()
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/LICENSE
----------------------------------------------------------------------
diff --git a/LICENSE b/LICENSE
index d3d076a..879a1d3 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1327,6 +1327,31 @@ If you have downloaded a copy of the RapidJSON binary from Tencent, please note
If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms. Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license.
A copy of the MIT License is included in this file.
+This product bundles RTIMULib2, which is offered under the license, below:
+
+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 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.
+
+
Other dependencies and licenses:
Open Source Software Licensed Under the BSD License:
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/ExtensionHeader.txt
----------------------------------------------------------------------
diff --git a/extensions/ExtensionHeader.txt b/extensions/ExtensionHeader.txt
new file mode 100644
index 0000000..8deafa5
--- /dev/null
+++ b/extensions/ExtensionHeader.txt
@@ -0,0 +1,26 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements. See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership. The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License. You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied. See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+cmake_minimum_required(VERSION 2.6)
+
+
+set(CMAKE_EXE_LINKER_FLAGS "-Wl,--export-all-symbols")
+set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--export-symbols")
+
+include_directories(../../libminifi/include ../../libminifi/include/core/yaml ../../libminifi/include/core ../../thirdparty/spdlog-20170710/include ../../thirdparty/concurrentqueue ../../thirdparty/yaml-cpp-yaml-cpp-0.5.3/include ${CIVET_THIRDPARTY_ROOT}/include ../../thirdparty/)
\ No newline at end of file
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/CMakeLists.txt
----------------------------------------------------------------------
diff --git a/extensions/sensors/CMakeLists.txt b/extensions/sensors/CMakeLists.txt
new file mode 100644
index 0000000..5e5bf64
--- /dev/null
+++ b/extensions/sensors/CMakeLists.txt
@@ -0,0 +1,56 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements. See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership. The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License. You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied. See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+include(${CMAKE_SOURCE_DIR}/extensions/ExtensionHeader.txt)
+
+include_directories(../../thirdparty/RTIMULib/RTIMULib)
+file(GLOB SOURCES "*.cpp")
+
+add_library(minifi-sensors STATIC ${SOURCES})
+
+set_property(TARGET minifi-sensors PROPERTY POSITION_INDEPENDENT_CODE ON)
+if(THREADS_HAVE_PTHREAD_ARG)
+ target_compile_options(PUBLIC minifi-sensors "-pthread")
+endif()
+if(CMAKE_THREAD_LIBS_INIT)
+ target_link_libraries(minifi-sensors "${CMAKE_THREAD_LIBS_INIT}")
+endif()
+
+target_link_libraries(minifi-sensors ${LIBMINIFI} )
+
+if (WIN32)
+ set_target_properties(minifi-sensors PROPERTIES
+ LINK_FLAGS "/WHOLEARCHIVE"
+ )
+elseif (APPLE)
+ set_target_properties(minifi-sensors PROPERTIES
+ LINK_FLAGS "-Wl,-all_load"
+ )
+else ()
+ set_target_properties(minifi-sensors PROPERTIES
+ LINK_FLAGS "-Wl,--whole-archive"
+ )
+endif ()
+
+add_dependencies(minifi-sensors RTIMULib)
+target_link_libraries(minifi-sensors RTIMULib)
+
+SET (SENSOR-EXTENSIONS minifi-sensors PARENT_SCOPE)
+
+register_extension(minifi-sensors)
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/GetEnvironmentalSensors.cpp
----------------------------------------------------------------------
diff --git a/extensions/sensors/GetEnvironmentalSensors.cpp b/extensions/sensors/GetEnvironmentalSensors.cpp
new file mode 100644
index 0000000..7aec8eb
--- /dev/null
+++ b/extensions/sensors/GetEnvironmentalSensors.cpp
@@ -0,0 +1,155 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <regex.h>
+#include <memory>
+#include <algorithm>
+#include <cctype>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <map>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "utils/ByteArrayCallback.h"
+#include "core/FlowFile.h"
+#include "core/logging/Logger.h"
+#include "core/ProcessContext.h"
+#include "core/Relationship.h"
+#include "GetEnvironmentalSensors.h"
+#include "io/DataStream.h"
+#include "io/StreamFactory.h"
+#include "ResourceClaim.h"
+#include "utils/StringUtils.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+std::shared_ptr<utils::IdGenerator> GetEnvironmentalSensors::id_generator_ = utils::IdGenerator::getIdGenerator();
+
+const char *GetEnvironmentalSensors::ProcessorName = "GetEnvironmentalSensors";
+
+core::Relationship GetEnvironmentalSensors::Success("success", "All files are routed to success");
+
+void GetEnvironmentalSensors::initialize() {
+ logger_->log_trace("Initializing EnvironmentalSensors");
+ // Set the supported properties
+ std::set<core::Property> properties;
+
+ setSupportedProperties(properties);
+ // Set the supported relationships
+ std::set<core::Relationship> relationships;
+ relationships.insert(Success);
+ setSupportedRelationships(relationships);
+}
+
+void GetEnvironmentalSensors::onSchedule(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSessionFactory> &sessionFactory) {
+ SensorBase::onSchedule(context, sessionFactory);
+
+ humidity_sensor_ = RTHumidity::createHumidity(&settings);
+ if (humidity_sensor_) {
+ humidity_sensor_->humidityInit();
+ } else {
+ throw std::runtime_error("RTHumidity could not be initialized");
+ }
+
+ pressure_sensor_ = RTPressure::createPressure(&settings);
+ if (pressure_sensor_) {
+ pressure_sensor_->pressureInit();
+ } else {
+ throw std::runtime_error("RTPressure could not be initialized");
+ }
+
+}
+
+void GetEnvironmentalSensors::notifyStop() {
+ delete humidity_sensor_;
+ delete pressure_sensor_;
+}
+
+GetEnvironmentalSensors::~GetEnvironmentalSensors() {
+}
+
+void GetEnvironmentalSensors::onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) {
+
+ auto flow_file_ = session->create();
+
+ if (imu->IMURead()) {
+ RTIMU_DATA imuData = imu->getIMUData();
+ auto vector = imuData.accel;
+ std::string degrees = RTMath::displayDegrees("acceleration", vector);
+ flow_file_->addAttribute("ACCELERATION", degrees);
+ } else {
+ logger_->log_trace("IMURead returned false. Could not gather acceleration");
+ }
+
+ RTIMU_DATA data;
+
+ bool have_sensor = false;
+
+ if (humidity_sensor_->humidityRead(data)) {
+ if (data.humidityValid) {
+ have_sensor = true;
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(2) << data.humidity;
+ flow_file_->addAttribute("HUMIDITY", ss.str());
+ }
+ } else {
+ logger_->log_trace("Could not read humidity sensors");
+ }
+
+ if (pressure_sensor_->pressureRead(data)) {
+ if (data.pressureValid) {
+ have_sensor = true;
+ {
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(2) << data.pressure;
+ flow_file_->addAttribute("PRESSURE", ss.str());
+ }
+
+ if (data.temperatureValid) {
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(2) << data.temperature;
+ flow_file_->addAttribute("TEMPERATURE", ss.str());
+ }
+
+ }
+ } else {
+ logger_->log_trace("Could not read pressure sensors");
+ }
+
+ if (have_sensor) {
+
+ WriteCallback callback("GetEnvironmentalSensors");
+ session->write(flow_file_, &callback);
+ session->transfer(flow_file_, Success);
+ }
+
+}
+
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/GetEnvironmentalSensors.h
----------------------------------------------------------------------
diff --git a/extensions/sensors/GetEnvironmentalSensors.h b/extensions/sensors/GetEnvironmentalSensors.h
new file mode 100644
index 0000000..f18985a
--- /dev/null
+++ b/extensions/sensors/GetEnvironmentalSensors.h
@@ -0,0 +1,85 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef EXTENSIONS_SENSORS_GETENVIRONMENTALSENSORS_H_
+#define EXTENSIONS_SENSORS_GETENVIRONMENTALSENSORS_H_
+
+
+
+#include <memory>
+#include <regex>
+
+#include "utils/ByteArrayCallback.h"
+#include "FlowFileRecord.h"
+#include "core/Processor.h"
+#include "core/ProcessSession.h"
+#include "core/Core.h"
+#include "core/Property.h"
+#include "core/Resource.h"
+#include "utils/Id.h"
+#include "RTIMULib.h"
+#include "SensorBase.h"
+#include "RTMath.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+// EnvironmentalSensors Class
+class GetEnvironmentalSensors : public SensorBase {
+ public:
+
+ // Constructor
+ /*!
+ * Create a new processor
+ */
+ GetEnvironmentalSensors(std::string name, uuid_t uuid = NULL)
+ : SensorBase(name, uuid),
+ humidity_sensor_(nullptr),
+ pressure_sensor_(nullptr),
+ logger_(logging::LoggerFactory<GetEnvironmentalSensors>::getLogger()) {
+ }
+ // Destructor
+ virtual ~GetEnvironmentalSensors();
+ // Processor Name
+ static const char *ProcessorName;
+ static core::Relationship Success;
+ // Supported Properties
+
+ virtual void onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) override;
+ virtual void initialize() override;
+ virtual void onSchedule(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSessionFactory> &sessionFactory) override;
+
+ protected:
+ virtual void notifyStop();
+ private:
+ RTHumidity *humidity_sensor_;
+ RTPressure *pressure_sensor_;
+ std::shared_ptr<logging::Logger> logger_;
+ static std::shared_ptr<utils::IdGenerator> id_generator_;
+};
+
+REGISTER_RESOURCE(GetEnvironmentalSensors)
+
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
+#endif /* EXTENSIONS_SENSORS_GETENVIRONMENTALSENSORS_H_ */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/GetMovementSensors.cpp
----------------------------------------------------------------------
diff --git a/extensions/sensors/GetMovementSensors.cpp b/extensions/sensors/GetMovementSensors.cpp
new file mode 100644
index 0000000..f7cb0f0
--- /dev/null
+++ b/extensions/sensors/GetMovementSensors.cpp
@@ -0,0 +1,97 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <memory>
+#include <algorithm>
+#include <cctype>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <map>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "utils/ByteArrayCallback.h"
+#include "core/FlowFile.h"
+#include "core/logging/Logger.h"
+#include "core/ProcessContext.h"
+#include "core/Relationship.h"
+#include "GetMovementSensors.h"
+#include "io/DataStream.h"
+#include "io/StreamFactory.h"
+#include "ResourceClaim.h"
+#include "utils/StringUtils.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+const char *GetMovementSensors::ProcessorName = "GetMovementSensors";
+
+core::Relationship GetMovementSensors::Success("success", "All files are routed to success");
+
+void GetMovementSensors::initialize() {
+ logger_->log_trace("Initializing GetMovementSensors");
+ // Set the supported properties
+ std::set<core::Property> properties;
+
+ setSupportedProperties(properties);
+ // Set the supported relationships
+ std::set<core::Relationship> relationships;
+ relationships.insert(Success);
+ setSupportedRelationships(relationships);
+}
+
+GetMovementSensors::~GetMovementSensors() {
+}
+
+void GetMovementSensors::onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) {
+ auto flow_file_ = session->create();
+
+ if (imu->IMURead()) {
+ RTIMU_DATA imuData = imu->getIMUData();
+
+ if (imuData.accelValid) {
+ auto vector = imuData.accel;
+ std::string degrees = RTMath::displayDegrees("acceleration", vector);
+ flow_file_->addAttribute("ACCELERATION", degrees);
+ } else {
+ logger_->log_trace("Could not read accelerometer");
+ }
+ if (imuData.gyroValid) {
+ auto vector = imuData.gyro;
+ std::string degrees = RTMath::displayDegrees("gyro", vector);
+ flow_file_->addAttribute("GYRO", degrees);
+ } else {
+ logger_->log_trace("Could not read gyroscope");
+ }
+
+ WriteCallback callback("GetMovementSensors");
+ session->write(flow_file_, &callback);
+ session->transfer(flow_file_, Success);
+ }
+}
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/GetMovementSensors.h
----------------------------------------------------------------------
diff --git a/extensions/sensors/GetMovementSensors.h b/extensions/sensors/GetMovementSensors.h
new file mode 100644
index 0000000..e1c1f94
--- /dev/null
+++ b/extensions/sensors/GetMovementSensors.h
@@ -0,0 +1,77 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef EXTENSIONS_SENSORS_GETMOVEMENT_H_
+#define EXTENSIONS_SENSORS_GETMOVEMENT_H_
+
+
+
+#include <memory>
+#include <regex>
+
+#include "utils/ByteArrayCallback.h"
+#include "FlowFileRecord.h"
+#include "core/Processor.h"
+#include "core/ProcessSession.h"
+#include "core/Core.h"
+#include "core/Property.h"
+#include "core/Resource.h"
+#include "utils/Id.h"
+#include "SensorBase.h"
+#include "RTIMULib.h"
+#include "RTMath.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+// GetMovementSensors Class
+class GetMovementSensors : public SensorBase {
+ public:
+
+ // Constructor
+ /*!
+ * Create a new processor
+ */
+ GetMovementSensors(std::string name, uuid_t uuid = NULL)
+ : SensorBase(name, uuid),
+ logger_(logging::LoggerFactory<GetMovementSensors>::getLogger()) {
+ }
+ // Destructor
+ virtual ~GetMovementSensors();
+ // Processor Name
+ static const char *ProcessorName;
+ static core::Relationship Success;
+ // Supported Properties
+
+ virtual void onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) override;
+ virtual void initialize() override;
+
+ private:
+ std::shared_ptr<logging::Logger> logger_;
+};
+
+REGISTER_RESOURCE(GetMovementSensors)
+
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
+#endif /* EXTENSIONS_SENSORS_GETMOVEMENT_H_ */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/README.MD
----------------------------------------------------------------------
diff --git a/extensions/sensors/README.MD b/extensions/sensors/README.MD
new file mode 100644
index 0000000..9405558
--- /dev/null
+++ b/extensions/sensors/README.MD
@@ -0,0 +1,20 @@
+<!--
+ Licensed to the Apache Software Foundation (ASF) under one or more
+ contributor license agreements. See the NOTICE file distributed with
+ this work for additional information regarding copyright ownership.
+ The ASF licenses this file to You under the Apache License, Version 2.0
+ (the "License"); you may not use this file except in compliance with
+ the License. You may obtain a copy of the License at
+ http://www.apache.org/licenses/LICENSE-2.0
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+-->
+
+Sensors is intended to be an extension whereby we retrieve sensor data through various libraries within MiNiFi C++ Processors.
+
+Currently RTIMULib is used to gather environmental and movement based data and place them into flow files. As this package progresses, more
+libraries may be used to gather sensor data. In the current approach a single FlowFile is created when OnTrigger(..) is called. This may change
+at any time. Future updates should allow for decorating FlowFiles with attributes or referencing parent FlowFiles if necessary.
\ No newline at end of file
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/SensorBase.cpp
----------------------------------------------------------------------
diff --git a/extensions/sensors/SensorBase.cpp b/extensions/sensors/SensorBase.cpp
new file mode 100644
index 0000000..0253bc6
--- /dev/null
+++ b/extensions/sensors/SensorBase.cpp
@@ -0,0 +1,75 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <memory>
+#include <algorithm>
+#include <cctype>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <map>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "utils/ByteArrayCallback.h"
+#include "core/FlowFile.h"
+#include "core/logging/Logger.h"
+#include "core/ProcessContext.h"
+#include "core/Relationship.h"
+#include "SensorBase.h"
+#include "io/DataStream.h"
+#include "io/StreamFactory.h"
+#include "ResourceClaim.h"
+#include "utils/StringUtils.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+core::Relationship SensorBase::Success("success", "All files are routed to success");
+
+void SensorBase::initialize() {
+}
+
+void SensorBase::onSchedule(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSessionFactory> &sessionFactory) {
+ imu = std::unique_ptr<RTIMU>(RTIMU::createIMU(&settings));
+ if (imu) {
+ imu->IMUInit();
+ imu->setGyroEnable(true);
+ imu->setAccelEnable(true);
+ } else {
+ throw std::runtime_error("RTIMU could not be initialized");
+ }
+
+}
+
+SensorBase::~SensorBase() {
+}
+
+void SensorBase::onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) {
+}
+
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/SensorBase.h
----------------------------------------------------------------------
diff --git a/extensions/sensors/SensorBase.h b/extensions/sensors/SensorBase.h
new file mode 100644
index 0000000..40990fe
--- /dev/null
+++ b/extensions/sensors/SensorBase.h
@@ -0,0 +1,92 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef EXTENSIONS_SENSORS_SENSORBASE_H_
+#define EXTENSIONS_SENSORS_SENSORBASE_H_
+
+
+
+#include <memory>
+#include <regex>
+
+#include "utils/ByteArrayCallback.h"
+#include "FlowFileRecord.h"
+#include "core/Processor.h"
+#include "core/ProcessSession.h"
+#include "core/Core.h"
+#include "core/Property.h"
+#include "core/Resource.h"
+#include "utils/Id.h"
+#include "RTIMULib.h"
+#include "RTMath.h"
+
+namespace org {
+namespace apache {
+namespace nifi {
+namespace minifi {
+namespace processors {
+
+// SensorBase Class
+class SensorBase : public core::Processor {
+ public:
+
+ // Constructor
+ /*!
+ * Create a new processor
+ */
+ SensorBase(std::string name, uuid_t uuid = NULL)
+ : Processor(name, uuid),
+ imu(nullptr),
+ logger_(logging::LoggerFactory<SensorBase>::getLogger()) {
+ }
+ // Destructor
+ virtual ~SensorBase();
+ // Processor Name
+ static core::Relationship Success;
+ // Supported Properties
+
+ virtual void onTrigger(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSession> &session) override;
+ virtual void initialize() override;
+ virtual void onSchedule(const std::shared_ptr<core::ProcessContext> &context, const std::shared_ptr<core::ProcessSessionFactory> &sessionFactory) override;
+
+ class WriteCallback : public OutputStreamCallback {
+ public:
+ WriteCallback(std::string data)
+ : _data(const_cast<char*>(data.data())),
+ _dataSize(data.size()) {
+ }
+ char *_data;
+ uint64_t _dataSize;
+ int64_t process(std::shared_ptr<io::BaseStream> stream) {
+ int64_t ret = 0;
+ if (_data && _dataSize > 0)
+ ret = stream->write(reinterpret_cast<uint8_t*>(_data), _dataSize);
+ return ret;
+ }
+ };
+ protected:
+ RTIMUSettings settings;
+ std::unique_ptr<RTIMU> imu;
+ std::shared_ptr<logging::Logger> logger_;
+};
+
+} /* namespace processors */
+} /* namespace minifi */
+} /* namespace nifi */
+} /* namespace apache */
+} /* namespace org */
+#endif /* EXTENSIONS_SENSORS_SENSORBASE_H_ */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/SensorLoader.cpp
----------------------------------------------------------------------
diff --git a/extensions/sensors/SensorLoader.cpp b/extensions/sensors/SensorLoader.cpp
new file mode 100644
index 0000000..4b54eee
--- /dev/null
+++ b/extensions/sensors/SensorLoader.cpp
@@ -0,0 +1,30 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "SensorLoader.h"
+
+#include "core/FlowConfiguration.h"
+
+bool SensorFactory::added = core::FlowConfiguration::add_static_func("createSensorFactory");
+
+extern "C" {
+
+void *createSensorFactory(void) {
+ return new SensorFactory();
+}
+
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/extensions/sensors/SensorLoader.h
----------------------------------------------------------------------
diff --git a/extensions/sensors/SensorLoader.h b/extensions/sensors/SensorLoader.h
new file mode 100644
index 0000000..db1d58c
--- /dev/null
+++ b/extensions/sensors/SensorLoader.h
@@ -0,0 +1,71 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef EXTENSIONS_ROCKSDBLOADER_H
+#define EXTENSIONS_ROCKSDBLOADER_H
+
+#include "core/ClassLoader.h"
+#include "GetEnvironmentalSensors.h"
+#include "GetMovementSensors.h"
+#include "utils/StringUtils.h"
+
+class __attribute__((visibility("default"))) SensorFactory : public core::ObjectFactory {
+ public:
+ SensorFactory() {
+
+ }
+
+ /**
+ * Gets the name of the object.
+ * @return class name of processor
+ */
+ virtual std::string getName() {
+ return "SensorFactory";
+ }
+
+ virtual std::string getClassName() {
+ return "SensorFactory";
+ }
+ /**
+ * Gets the class name for the object
+ * @return class name for the processor.
+ */
+ virtual std::vector<std::string> getClassNames() {
+ std::vector<std::string> class_names;
+ class_names.push_back("EnvironmentalSensors");
+ return class_names;
+ }
+
+ virtual std::unique_ptr<ObjectFactory> assign(const std::string &class_name) {
+ if (minifi::utils::StringUtils::equalsIgnoreCase(class_name, "GetEnvironmentalSensors")) {
+ return std::unique_ptr<ObjectFactory>(new core::DefautObjectFactory<minifi::processors::GetEnvironmentalSensors>());
+ } else if (minifi::utils::StringUtils::equalsIgnoreCase(class_name, "GetMovementSensors")) {
+ return std::unique_ptr<ObjectFactory>(new core::DefautObjectFactory<minifi::processors::GetMovementSensors>());
+ } else {
+ return nullptr;
+ }
+ }
+
+ static bool added;
+
+}
+;
+
+extern "C" {
+void *createSensorFactory(void);
+}
+#endif /* EXTENSIONS_ROCKSDBLOADER_H */
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/libminifi/test/resources/TestEnvironmental.yml
----------------------------------------------------------------------
diff --git a/libminifi/test/resources/TestEnvironmental.yml b/libminifi/test/resources/TestEnvironmental.yml
new file mode 100644
index 0000000..5e2f474
--- /dev/null
+++ b/libminifi/test/resources/TestEnvironmental.yml
@@ -0,0 +1,71 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements. See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership. The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License. You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied. See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+Flow Controller:
+ name: MiNiFi Flow
+ id: 2438e3c8-015a-1000-79ca-83af40ec1990
+Processors:
+ - name: pcap
+ id: 2438e3c8-015a-1000-79ca-83af40ec1991
+ class: org.apache.nifi.processors.standard.GetEnvironmentalSensors
+ max concurrent tasks: 1
+ scheduling strategy: TIMER_DRIVEN
+ scheduling period: 10 msec
+ penalization period: 1 sec
+ yield period: 10 msec
+ run duration nanos: 0
+ auto-terminated relationships list:
+ Properties:
+ - name: LogAttribute
+ id: 2438e3c8-015a-1000-79ca-83af40ec1992
+ class: org.apache.nifi.processors.standard.LogAttribute
+ max concurrent tasks: 1
+ scheduling strategy: TIMER_DRIVEN
+ scheduling period: 1 sec
+ penalization period: 30 sec
+ yield period: 1 sec
+ run duration nanos: 0
+ auto-terminated relationships list: response
+ Properties:
+ Log Level: info
+ Log Payload: true
+
+Connections:
+ - name: TransferFilesToRPG
+ id: 2438e3c8-015a-1000-79ca-83af40ec1997
+ source name: pcap
+ source id: 2438e3c8-015a-1000-79ca-83af40ec1991
+ source relationship name: success
+ destination name: LogAttribute
+ destination id: 2438e3c8-015a-1000-79ca-83af40ec1992
+ max work queue size: 0
+ max work queue data size: 1 MB
+ flowfile expiration: 60 sec
+ - name: TransferFilesToRPG2
+ id: 2438e3c8-015a-1000-79ca-83af40ec1917
+ source name: LogAttribute
+ source id: 2438e3c8-015a-1000-79ca-83af40ec1992
+ destination name: LogAttribute
+ destination id: 2438e3c8-015a-1000-79ca-83af40ec1992
+ source relationship name: success
+ max work queue size: 0
+ max work queue data size: 1 MB
+ flowfile expiration: 60 sec
+
+Remote Processing Groups:
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/libminifi/test/sensors-tests/CMakeLists.txt
----------------------------------------------------------------------
diff --git a/libminifi/test/sensors-tests/CMakeLists.txt b/libminifi/test/sensors-tests/CMakeLists.txt
new file mode 100644
index 0000000..767fd57
--- /dev/null
+++ b/libminifi/test/sensors-tests/CMakeLists.txt
@@ -0,0 +1,42 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements. See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership. The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License. You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied. See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+file(GLOB SENSORS_TESTS "*.cpp")
+
+SET(SENSORS_INT_TEST_COUNT 0)
+
+FOREACH(testfile ${SENSORS_TESTS})
+ get_filename_component(testfilename "${testfile}" NAME_WE)
+ add_executable("${testfilename}" "${testfile}" )
+ target_include_directories(${testfilename} PRIVATE BEFORE "${CMAKE_SOURCE_DIR}/libminifi/test/")
+ target_include_directories(${testfilename} PRIVATE BEFORE "${CMAKE_SOURCE_DIR}/thirdparty/RTIMULib/RTIMULib/")
+ target_include_directories(${testfilename} PRIVATE BEFORE "${CMAKE_SOURCE_DIR}/extensions/sensors/")
+ target_include_directories(${testfilename} PRIVATE BEFORE "${CMAKE_BINARY_DIR}/extensions/sensors/")
+ createTests("${testfilename}")
+ if(APPLE)
+ target_link_libraries ("${testfilename}" -Wl,-all_load minifi-sensors )
+ else()
+ target_link_libraries ("${testfilename}" -Wl,--whole-archive minifi-sensors -Wl,--no-whole-archive)
+ endif()
+ MATH(EXPR SENSORS_INT_TEST_COUNT "${SENSORS_INT_TEST_COUNT}+1")
+ENDFOREACH()
+
+message("-- Finished building ${SENSORS_INT_TEST_COUNT} sensor(s) test file(s)...")
+add_test(NAME SensorTests COMMAND SensorTests "${TEST_RESOURCES}/TestEnvironmental.yml" "${TEST_RESOURCES}/")
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/libminifi/test/sensors-tests/SensorTests.cpp
----------------------------------------------------------------------
diff --git a/libminifi/test/sensors-tests/SensorTests.cpp b/libminifi/test/sensors-tests/SensorTests.cpp
new file mode 100644
index 0000000..6c995d5
--- /dev/null
+++ b/libminifi/test/sensors-tests/SensorTests.cpp
@@ -0,0 +1,100 @@
+/**
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <sys/stat.h>
+#undef NDEBUG
+#include <cassert>
+#include <utility>
+#include <chrono>
+#include <fstream>
+#include <memory>
+#include <string>
+#include <thread>
+#include <type_traits>
+#include <vector>
+#include <iostream>
+#include <sstream>
+#include "../TestBase.h"
+#include "utils/StringUtils.h"
+#include "core/Core.h"
+#include "core/logging/Logger.h"
+#include "core/ProcessGroup.h"
+#include "core/yaml/YamlConfiguration.h"
+#include "FlowController.h"
+#include "properties/Configure.h"
+#include "../unit/ProvenanceTestHelper.h"
+#include "io/StreamFactory.h"
+#include "core/ConfigurableComponent.h"
+#include "../integration/IntegrationBase.h"
+#include "GetEnvironmentalSensors.h"
+
+class PcapTestHarness : public IntegrationBase {
+ public:
+ PcapTestHarness() {
+ char format[] = "/tmp/ssth.XXXXXX";
+ dir = testController.createTempDirectory(format);
+ }
+
+ void testSetup() {
+ LogTestController::getInstance().setTrace<minifi::processors::GetEnvironmentalSensors>();
+ LogTestController::getInstance().setDebug<minifi::FlowController>();
+ LogTestController::getInstance().setDebug<minifi::SchedulingAgent>();
+ LogTestController::getInstance().setDebug<minifi::core::ProcessGroup>();
+ LogTestController::getInstance().setDebug<minifi::core::Processor>();
+ LogTestController::getInstance().setDebug<minifi::ThreadedSchedulingAgent>();
+ }
+
+ void cleanup() {
+ LogTestController::getInstance().reset();
+ }
+
+ void runAssertions() {
+ assert(LogTestController::getInstance().contains("Initializing EnvironmentalSensors") == true);
+ }
+
+ void queryRootProcessGroup(std::shared_ptr<core::ProcessGroup> pg) {
+ std::shared_ptr<core::Processor> proc = pg->findProcessor("pcap");
+ assert(proc != nullptr);
+
+ auto inv = std::dynamic_pointer_cast<minifi::processors::GetEnvironmentalSensors>(proc);
+ assert(inv != nullptr);
+
+ configuration->set("nifi.c2.enable", "false");
+ }
+
+ protected:
+ char *dir;
+ TestController testController;
+};
+
+int main(int argc, char **argv) {
+ std::string key_dir, test_file_location, url;
+
+ if (argc > 1) {
+ test_file_location = argv[1];
+ }
+
+
+ PcapTestHarness harness;
+
+ harness.setKeyDir(key_dir);
+
+ harness.run(test_file_location);
+
+ return 0;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/.gitignore
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/.gitignore b/thirdparty/RTIMULib/.gitignore
new file mode 100644
index 0000000..f66f6a5
--- /dev/null
+++ b/thirdparty/RTIMULib/.gitignore
@@ -0,0 +1,53 @@
+Makefile*
+*.autosave
+*.opensdf
+*.pro.user
+*.cache
+Makefile*
+!Linux/RTIMULibDrive/Makefile
+!Linux/RTIMULibDrive10/Makefile
+!Linux/RTIMULibDrive11/Makefile
+!Linux/RTIMULibCal/Makefile
+!Linux/RTIMULibvrpn/Makefile
+build/
+debug/
+Debug/
+Release/
+ipch/
+release/
+objects/
+Output/
+GeneratedFiles/
+*.deps
+*.log.*
+*.exe
+*.lib
+*.ilk
+*.pdb
+*.log
+*.ncb
+*.user
+*.suo
+*.aps
+*.sdf
+*.obj
+*.msi
+*.pch
+*.ini
+*.srf
+*.srx
+*.dta
+*~
+*.a
+*.o
+*.axf
+*.bin
+*.map
+!.gitignore
+.metadata/
+*.dat
+*.tgz
+*.bz2
+*.pro.user*
+.DS_store
+octave-core
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/LICENSE
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/LICENSE b/thirdparty/RTIMULib/LICENSE
new file mode 100644
index 0000000..ff9ab1f
--- /dev/null
+++ b/thirdparty/RTIMULib/LICENSE
@@ -0,0 +1,29 @@
+///////////////////////////////////////////////////////////
+//
+// 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.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/README.md
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/README.md b/thirdparty/RTIMULib/README.md
new file mode 100644
index 0000000..d446157
--- /dev/null
+++ b/thirdparty/RTIMULib/README.md
@@ -0,0 +1,370 @@
+# RTIMULib - a versatile C++ and Python 9-dof, 10-dof and 11-dof IMU library
+
+RTIMULib is the simplest way to connect a 9-dof, 10-dof or 11-dof IMU to an embedded Linux system and obtain Kalman-filtered quaternion or Euler angle pose data. Basically, two simple funtion calls (IMUInit() and IMURead()) are pretty much all that's needed to integrate RTIMULib.
+
+## Have questions, need help or want to comment?
+
+Please use the richards-tech user forum at https://groups.google.com/forum/#!forum/richards-tech-user-forum.
+
+## Features
+
+The Linux directory contains the main demo apps for embeeded Linux systems:
+
+* RTIMULibDrive is a simple app that shows to to use the RTIMULib library in a basic way.
+* RTIMULibDrive10 adds support for pressure/temperature sensors.
+* RTIMULibDrive11 adds support for pressure/temperature/humidity sensors.
+* RTIMULibCal is a command line calibration tool for the magnetometers and accelerometers.
+* RTIMULibvrpn shows how to use RTIMULib with vrpn.
+* RTIMULibDemo is a simple GUI app that displays the fused IMU data in real-time.
+* RTIMULibDemoGL adds OpenGL visualization to RTIMULibDemo.
+
+RTIMULib is a C++ library but there are also Python bindings in Linux/python. It's easy to build and install the Python RTIMULib library using the provided setup.py after which any Python script will have access to RTIMULib functionality. See Linux/python.README.md (https://github.com/richards-tech/RTIMULib/blob/master/Linux/python/README.md) for more details. Two demo scripts show how to use the Python interface.
+
+Check out www.richards-tech.com for more details, updates and news.
+
+RTIMULib currently supports the following IMUs:
+
+* InvenSense MPU-9150 single chip IMU.
+* InvenSense MPU-6050 plus HMC5883 magnetometer on MPU-6050's aux bus (handled by the MPU-9150 driver).
+* InvenSense MPU-6050 gyros + acclerometers. Treated as MPU-9150 without magnetometers.
+* InvenSense MPU-9250 single chip IMU (I2C and SPI).
+* STM LSM9DS0 single chip IMU.
+* STM LSM9DS1 single chip IMU.
+* L3GD20H + LSM303D (optionally with the LPS25H) as used on the Pololu AltIMU-10 v4.
+* L3GD20 + LSM303DLHC as used on the Adafruit 9-dof (older version with GD20 gyro) IMU.
+* L3GD20H + LSM303DLHC (optionally with BMP180) as used on the new Adafruit 10-dof IMU.
+* Bosch BMX055 (although magnetometer support is experimental currently).
+* Bosch BNO055 IMU with onchip fusion. Note: will not work reliably with RaspberryPi/Pi2 due to clock-stretching issues.
+
+The LSM9DS1 implementation was generously supplied by XECDesign.
+
+Pressure/temperature sensing is supported for the following pressure sensors:
+
+* BMP180
+* LPS25H
+* MS5611
+* MS5637
+
+Humidity/temperature sensing is supported for the following humidity sensors:
+
+* HTS221
+* HTU21D
+
+The humidity infrastructure and HTS221 support was generously supplied by XECDesign. It follows the model used by the pressure infrastructure - see RTIMULibDrive11 for an example of how to use this.
+
+Note that currently only pressure and humidity sensors connected via I2C are supported. Also, an MS5637 sensor will be auto-detected as an MS5611. To get the correct processing for the MS5637, edit the RTIMULib.ini file and set PressureType=5.
+
+By default, RTIMULib will try to autodiscover IMUs, pressure and humidity sensors on I2C and SPI busses (only IMUs on the SPI bus). This will use I2C bus 1 and SPI bus 0 although this can be changed by hand editing the .ini settings file (usually called RTIMULib.ini) loaded/saved in the current working directory by any of the RTIMULib apps. RTIMULib.ini is self-documenting making it easy to edit. Alternatively, RTIMULibDemo and RTIMULibDemoGL provide a GUI interface for changing some of the major settings in the .ini file.
+
+RTIMULib also supports multiple sensor integration fusion filters such as Kalman filters.
+
+Two types of platforms are supported:
+
+* Embedded Linux. RTIMULib is supported for the Raspberry Pi (Raspbian) and Intel Edison. Demo apps for these can be found in the Linux directory and instructions for building and running can be found there. Its prerequisites are very simple - just I2C support on the target system along with the standard build-essential (included in the Raspberry Pi Raspbian distribution by default).
+
+* Desktop (Ubuntu/Windows/Mac). There are two apps (RTHostIMU and RTHostIMUGL) that allow the sensor fusion to be separated from the sensor interfacing and data collection. An Arduino (running the RTArduLinkIMU sketch from the RTIMULib-Arduino repo) fitted with an IMU chip collects the sensor data and sends it to the desktop. RTHostIMU and RTHostIMUGL (this one has an OpenGL visualization of the data) communicate with the Arduino via a USB connection.
+
+The MPU-9250 and SPI driver code is based on code generously supplied by staslock@gmail.com (www.clickdrive.io). I am sure that any bugs that may exist are due to my integration efforts and not the quality of the supplied code!
+
+RTIMULib is licensed under the MIT license.
+
+## Repo structure
+
+### RTIMULib
+
+This is the actual RTIMULib library source. Custom apps only need to include this library.
+
+### Linux
+
+This directory contains the embedded Linux demo apps (for Raspberry Pi and Intel Edison) and also the Python interface to RTIMULib.
+
+### RTHost
+
+RTHost contains the two apps, RTHost and RTHostGL, that can be used by desktops that don't have direct connection to an IMU (as they don't have I2C or SPI interfaces). An Arduino running RTArduLinkIMU from the RTIMULib-Arduino repo provides the hardware interface and a USB cable provides the connection between the desktop and the Arduino.
+
+### RTEllipsoidFit
+
+This contains Octave code used by the ellipsiod fit data generation in RTIMULibCal, RTIMULibDemo, RTIMULibDemoGL, RTHostIMU and RTHostIMUGL. It's important that a copy of this directory is at the same level, or the one above, the app's working directory or ellipsoid fit data generation will fail.
+
+## Note about magnetometer (compass) calibration
+
+It is essential to calibrate the magnetometer or else very poor fusion results will be obtained. For more about this, see http://wp.me/p4qcHg-b4. RTIMULibDemo (GUI) and RTIMULibCal (command line) can be used to do this. They both support magnetometer min/max, magnetometer ellipsoid fit and accelerometer min/max calibration.
+
+Also, if using a non-standard axis rotation (see http://wp.me/p4qcHg-cO), magnetometer calibration (and accelerometer calibration if that has been performed) MUST be run AFTER changing the axis rotation.
+
+## Next Steps
+
+SyntroPiNav (an app for the Raspberry Pi) and SyntroNavView can be used as a convenient system to experiment with IMU chips, drivers and filters. SyntroPiNav runs on the Pi and transmits IMU data along with filter outputs over a LAN to SyntroNavView running on an Ubuntu PC. SyntroNavView also displays the data and provides a 3D graphics view of the calculated pose.
+
+Since all IMU data is sent to SyntroNavView, SyntroNavView can run its own local filter. This makes it a very convenient testbed for new filter development as the speed of the desktop can be used to accelerate implementation and testing. When ready, the updated RTIMULib can be compiled into SyntroPiNav and should work exactly the same as on the desktop.
+
+SyntroPiNav is available as part of the richards-tech SyntroPiApps repo (https://github.com/richards-tech/SyntroPiApps) while SyntroNavView is available as part of the richards-tech SyntroApps repo (https://github.com/richards-tech/SyntroApps).
+
+## Release history
+
+### June 22 2015 - 7.2.1
+
+Improvement to Linux CMakeLists.txt. Added temp comp to HTU21D.
+
+### June 21 2015 - 7.2.0
+
+Added support for HTU21D humidity sensor.
+
+### June 17 2015 - 7.1.0
+
+Added humidity support to the demo apps and created RTIMULibDrive11 and Fusion11.py. Fixed some related build problems. Updated some source headers.
+
+### June 15 2015 - 7.0.3
+
+Improved CMake versioning system.
+
+### June 12 2015 - 7.0.2
+
+Also added SOVERSION to RTIMULibGL build via CMake. Note - on Linux it may be necessary to run "sudo ldconfig" after building.
+
+### June 12 2015 - 7.0.1
+
+Added SOVERSION to CMake build library.
+
+### June 9 2015 - 7.0.0
+
+New humidity infrastructure and HTS221 support added. Thanks to XECDesign for this. Due to lack of hardware for testing at this time, this release is somewhat experimental - use 6.3.0 if problems are encountered.
+
+LSM9DS1 support added.
+
+### May 17 2015 - 6.3.0
+
+Added support for the BNO055. The BNO055 always uses its onchip fusion rather than RTIMULib filters. This will not work reliably with the Raspberry Pi/Pi2 due to clock-stretching issues.
+
+### April 30 2015 - 6.2.1
+
+Added second order temperature compensation for MS5611 and MS5637. MS5637 still seems to be affected by temperature - this is being investigated.
+
+### April 24 2015 - 6.2.0
+
+Add support for Bosch BMX055 IMU and MS5637 pressure sensor. See notes above about auto-detection for the MS5637.
+
+The BMX055 implementation is slightly experimental as the magnetometer results show significant asymmetry about zero. The processing of the magnetometer data is fairly complex and there could be an error in it. Calibration is essential for this IMU at the moment.
+
+### March 31 2015 - 6.1.0
+
+Allow RTQF Slerp power to be changed while running while fusion running. Some performance improvements and cleanups.
+
+### March 29 2015 - 6.0.0
+
+Changed RTQF state correction mechanism to use quaternion SLERP. This is a little experimental - if you encounter problems, please use the 5.6.0 release (from the Releases tab).
+
+### March 21 2015 - 5.6.0
+
+Added support for MPU6050 + HMC5883 IMUs (HMC5883 on MPU-6050's aux bus).
+
+### March 20 2015 - 5.5.0
+
+Added support for the MS5611 pressure sensor and also modified MPU-9150 driver to also support the MPU-6050.
+
+### February 21 2015 - 5.4.0
+
+Python API now works with Python 2.7 and Python 3.4.
+
+Changed MPU9150 and MPU9250 drivers so that compass adjust is performed before axis swap.
+
+### January 31 2015 - 5.3.0
+
+Added abilty to set magnetic declination in the .ini file. This value in radians is subtracted from the measured heading before being used by the fusion algorithm.
+
+### January 24 2015 - 5.2.3
+
+Fixed problem with CMakeLists.txt for RTIMULibGL.
+
+### January 19 2015 - 5.2.2
+
+Improved some CMakeLists. RTIMULib can now be built with cmake independently.
+
+### December 29 2014 - 5.2.1
+
+Some improvements to the RTHost CMakelists.txt. Changed Visual Studio version to VS2013.
+
+### December 29 2014 - 5.2.0
+
+Added support for vrpn. There is a new demo app, RTIMULibvrpn, that shows how this works.
+
+RTSettings constructor now optionally allows the directory used for the .ini settings file to be specified. The original constructor uses the working directory whereas the additional constructor allows this bevaiour to be overridden.
+
+Changed install directory to /usr/local/bin when using the supplid Makefiles and qmake instead of /usr/bin. This is to be consistent with cmake-generated makefiles.
+
+### December 15 2014 - 5.1.0
+
+Added support for the LPS25H pressure/temperature sensor.
+
+Addeed support for SPI chip select 1 in addition to chip select 0. A new field, SPISelect, has been added. RTIMULib will try to autodetect SPI bus 0, select 0 and SPI bus 0 select 1 but others can be used if the RTIMULib.ini file is hand-edited.
+
+### December 10 2014 - 5.0.0
+
+Top level directory structure completely reorganized.
+
+Support for pressure sensors.
+
+New demo app, RTIMULibDrive10, to demonstrate use of pressure sensors with RTIMULib.
+
+RTIMULibDemo and RTIMULibDemoGL upgraded to support pressure sensors.
+
+Python library upgraded to support pressure sensors. A new demo script, Fusion10.py, shows how to use the interface.
+
+### December 3 2014 - 4.4.0
+
+Added RTIMULibDemoGL and reorganized the OpenGL components.
+
+### December 3 2014 - 4.3.2
+
+CMake build system now works for the RTHostIMU and RTHostIMUGL apps on Windows and Mac OS X. The full set of apps are built on Linux.
+
+Some minor driver fixes.
+
+RTHostIMUGL now runs on the Raspberry Pi. Simpler (non-ADS) shading is used to imporve performance.
+
+### December 2 2014 - 4.3.1
+
+Fixed the CMakeLists.txt for RTIMULibDemo.
+
+### December 2 2014 - 4.3.0
+
+Added cmake support (see build instructions for more info). This was based on work by Moritz Fischer at ettus.com. As part of this, the qextserialport folder was renamed to RTSerialPort. There are also some small fixes in the MPU-9150/9250 and GD20HM303D drivers.
+
+### November 18 2014 - 4.2.0
+
+Add the IMU axis rotation capability to better handle IMUs in non-standard orientations. See http://wp.me/p4qcHg-cO for more details of how to use this capability.
+
+### November 14 2014 - 4.1.0
+
+Corrected some problems with the continuous gyro bias update system. There is a new function in RTIMU called setGyroContinuousLearningAlpha() that allows the continuous learning rate to be set. RTIMULIB uses a rapid learning rate to collect the initial gyro bias data but then uses a much slower rate for continuous tracking of bias. This function allows the rate to be set if necessary - values can be between 0.0 and 1.0. Setting it to 0.0 turns off continuous learning completely so that gyro bias calculation only occurs during the rapid learning period.
+
+loadSettings() and saveSettings() in RTSettings.cpp are now virtual to give more flexibility in how settings are stored.
+
+### November 8 2014 - 4.0.1
+
+Fixed some missing MPU-9250 related defs in python interface.
+
+### November 7 2014 - 4.0.0
+
+Restructured library to add support for the MPU-9250 and SPI bus. This is a little experimental right now - use V3.1.1 if problems are encountered with existing supported IMUs. The MPU-9250 has been seen to hang when used on the SPI bus at sample rates above 300 samples per second. However, sample rates up to 1000 seem to work fine using I2C.
+
+The RTIMULib apps are now able to auto-detect on the I2C and SPI bus so, if only one supported IMU is present on either bus, the code should find it. Note that only the MPU-9250 is supported by the SPI driver at the moment. There are some new settings in the RTIMULib.ini file related to the SPI bus that may need editing in some systems. The default SPI bus is set 0 which works nicely for the Raspberry Pi. Connect the MPU-9250 to SPI0 and CS0 and it should work without needing to change anythin in RTIMULib.ini.
+
+### November 4 2014 - 3.1.1
+
+Can now supply the .ini name as a command line argument. For example:
+
+ RTIMULibCal Palm
+
+would calibrate a settings file called Palm.ini.
+
+### November 1 2014 - 3.1.0
+
+Added the RTIMULibCal application. This implements IMU calibration in no GUI (command line) environments.
+
+### October 13 2014 - 3.0.3
+
+Increased time allowed for ellipse fitting to complete before declaring it finished.
+
+### October 13 2014 - 3.0.2
+
+Added license information to Calibrate.pdf.
+
+### October 13 2014 - 3.0.1
+
+Fixed missing license header in RTEllipsoidFit.
+
+### October 13 2014 - 3.0.0
+
+RTIMULib now support accelerometer calibration and enhanced magnetometer calibration using ellipsoid fitting. Please check the Calibration.pdf document for instructions on how to create the calibration data.
+
+### October 8 2014 - 2.1.2
+
+Fixed some missed license header changes.
+
+### October 8 2014 - 2.1.1
+
+Fixed bug where the first com put was missed on the GUI dropdown in RTHostIMU and RTHostIMUGL.
+
+### October 8 2014 - 2.1.0
+
+Changed license to MIT.
+
+Added RTHostIMU and RTHostIMUGL. These apps use RTArduLink to connect the host system to an IMU connected to an Arduino. This allows processing to be split between the Arduino and the host system. Sensor data collection is performed on the Arduino, sensor fusion and display is performed on the host. This means that the apps will run on hosts without I2C ports (such as PCs). See below for more details.
+
+### October 2 2014 - 2.0.0
+
+Changed the gyro bias calculation to run automatically when the IMU is detected as being stable. This means
+that the IMU no longer needs to be kept still for 5 seconds after restart and gyro bias is continually tracked. IMUGyroBiasValid can be called to check if enough stable samples have been obtained for a reasonable bias calculation to be made. If the IMU is stable, this will normally occur within 5 seconds. If not stable at the start, it may take longer but it will occur eventually once enough stable samples have been obtained. If RTIMULibDemo never indicates a valid bias, the #defines RTIMU_FUZZY_GYRO_ZERO and/or RTIMU_FUZZY_ACCEL_ZERO may need to be increased if the gyro bias or accelerometer noise is unusually high. These should be set to be greater than the readings observed using RTIMULibDemo when the IMU is completely stable. In the case of the gyros, this should be the absolute values when the IMU isn't being moved. In the case of the accels, this should be the maximum change in values when the IMU isn't being moved.
+
+Stable gyro bias values are saved to the RTIMULib.ini file in order to speed up restarts. The values will once again be
+updated after enough stable samples have been obtained in order to track long term changes in gyro bias.
+
+If problems are encountered, try version 1.0.4 which is available under the GitHub repo releases tab. Please also report any issues via the GitHub issue system to help improve RTIMULib!
+
+### September 3 2014 - 1.0.4
+
+Fixed message error in RTIMUSettings.
+
+### September 2 2014 - 1.0.3
+
+CompassCalMax was returning m_compassCalMin in PyRTIMU_settings.cpp - changed to max instead. Thanks to Stefan Grufman for finding that.
+
+### August 6 2014 - 1.0.2
+
+Added missing compass sample rate defines for LSM303DLHC and updated settings comments. Thanks to Bill Robertson (broberts4) for spotting that!
+
+### July 29 2014 - 1.0.1
+
+Fixed the python getIMUData function.
+
+### July 7 2014 - 1.0.0
+
+#### Added python bindings
+
+Thanks to avishorp for the python code and Luke Heidelberger for a bug fix.
+
+### April 13 2014 - 0.9.4
+
+#### Added new RTQF fusion filter
+
+RTQF is a very highly stripped down Kalman filter that avoids matrix inversion and lot of other matrix operations. It makes a small performance difference on the Raspberry Pi but would have more impact on lower powered processors.
+
+### April 10 2014 - 0.9.3
+
+#### STM LSM9DS0 IMU Implementation now working
+
+The single chip IMU LSM9DS0 is now working with RTIMULib. An example breakout is available from Sparkfun - https://www.sparkfun.com/products/12636.
+
+### April 9 2014 - 0.9.2
+
+#### STM L3GD20H + LSM303D IMU Implementation now working
+
+The combination of the L3GD20H gyro and LSM303D accel/mag chip is now working. The physical configuration supported is as used on the Pololu Altimu V3 - http://www.pololu.com/product/2469. The pressure chip on the 10-dof version will be supported shortly but 9-dof is working now.
+
+#### STM L3GD20 + LSM303DLHC IMU Implementation now working
+
+The combination of the L3GD20 and LSM303DLHC accel/mag chip is now working. The physical configuration supported is as used on the Adafruit 9-dof IMU - http://www.adafruit.com/products/1714.
+
+### April 7 2014 - 0.9.1
+
+#### Improved performance with MPU-9150
+
+A new caching strategy for the MPU-9150 seems to be achieving 1000 samples per second without fifo overflows using a 900MHz Raspberry Pi and 400kHz I2C bus. This is as reported by RTIMULibDrive with a CPU utilization of 28%. RTIMULibDemo manages 890 samples per second with the MPU-9150 set to 1000 samples per second. The driver gracefully handles this situation although there is increased delay when the application cannot handle the full sample rate.
+
+#### Auto detection of IMU
+
+RTIMULib can now scan for supported IMUs and configure automatically. This is the default behavior now. It handles IMUs at alternate address automatically as well (for example, it will detect an MPU-9150 at 0x68 or 0x69).
+
+#### Partial support for STM L3GD20H/LSM303D IMUs
+
+This is in a very early state and only supports the gyro sensor at the moment.
+
+### April 4 2014 - 0.9.0
+
+Initial release with support for MPU9150.
+
+
+
+
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/CMakeLists.txt
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/CMakeLists.txt b/thirdparty/RTIMULib/RTIMULib/CMakeLists.txt
new file mode 100644
index 0000000..7d7e3d8
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/CMakeLists.txt
@@ -0,0 +1,97 @@
+#////////////////////////////////////////////////////////////////////////////
+#//
+#// This file is part of RTIMULib
+#//
+#// Copyright (c) 2014-2015, richards-tech
+#//
+#// 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 cmake support was based on work by Moritz Fischer at ettus.com.
+#// Original copyright notice:
+#
+# Copyright 2014 Ettus Research LLC
+#
+
+########################################################################
+IF(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
+ MESSAGE(FATAL_ERROR "Prevented in-tree built. This is bad practice.")
+ENDIF(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
+
+########################################################################
+# Project setup
+########################################################################
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8.9)
+PROJECT(RTIMULib CXX)
+ENABLE_TESTING()
+
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+INCLUDE(${CMAKE_CURRENT_SOURCE_DIR}/../RTIMULibVersion.txt)
+
+SET(LIBRTIMU_SRCS
+ RTFusion.cpp
+ RTFusionRTQF.cpp
+ RTMath.cpp
+ RTFusionKalman4.cpp
+ RTIMUAccelCal.cpp
+ RTIMUHal.cpp
+ RTIMUMagCal.cpp
+ RTIMUSettings.cpp
+ IMUDrivers/RTIMU.cpp
+ IMUDrivers/RTIMUGD20M303DLHC.cpp
+ IMUDrivers/RTIMUGD20HM303DLHC.cpp
+ IMUDrivers/RTIMUGD20HM303D.cpp
+ IMUDrivers/RTIMULSM9DS0.cpp
+ IMUDrivers/RTIMULSM9DS1.cpp
+ IMUDrivers/RTIMUMPU9150.cpp
+ IMUDrivers/RTIMUMPU9250.cpp
+ IMUDrivers/RTIMUBMX055.cpp
+ IMUDrivers/RTIMUBNO055.cpp
+ IMUDrivers/RTIMUNull.cpp
+ IMUDrivers/RTPressure.cpp
+ IMUDrivers/RTPressureBMP180.cpp
+ IMUDrivers/RTPressureLPS25H.cpp
+ IMUDrivers/RTPressureMS5611.cpp
+ IMUDrivers/RTPressureMS5637.cpp
+ IMUDrivers/RTHumidity.cpp
+ IMUDrivers/RTHumidityHTS221.cpp
+ IMUDrivers/RTHumidityHTU21D.cpp
+)
+
+IF(WIN32 AND QT5)
+ FIND_PACKAGE(Qt5Widgets)
+ FIND_PACKAGE(Qt5Gui)
+ qt5_wrap_ui(UI_HEADERS RTIMULibDemo.ui)
+ ADD_LIBRARY(RTIMULib STATIC ${LIBRTIMU_SRCS})
+ qt5_use_modules(RTIMULib Widgets Gui)
+ENDIF(WIN32 AND QT5)
+
+IF(WIN32 AND (NOT QT5))
+ FIND_PACKAGE(Qt4 REQUIRED)
+ INCLUDE(${QT_USE_FILE})
+ ADD_DEFINITIONS(${QT_DEFINITIONS})
+ ADD_LIBRARY(RTIMULib STATIC ${LIBRTIMU_SRCS})
+ TARGET_LINK_LIBRARIES(RTIMULib ${QT_LIBRARIES})
+ENDIF(WIN32 AND (NOT QT5))
+
+IF(UNIX)
+ ADD_LIBRARY(RTIMULib STATIC ${LIBRTIMU_SRCS})
+ SET_PROPERTY(TARGET RTIMULib PROPERTY VERSION ${RTIMULIB_VERSION})
+ SET_PROPERTY(TARGET RTIMULib PROPERTY SOVERSION ${RTIMULIB_VERSION_MAJOR})
+ENDIF(UNIX)
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.cpp
new file mode 100644
index 0000000..92f1854
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.cpp
@@ -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.
+
+
+#include "RTHumidity.h"
+
+#include "RTHumidityHTS221.h"
+#include "RTHumidityHTU21D.h"
+
+RTHumidity *RTHumidity::createHumidity(RTIMUSettings *settings)
+{
+ switch (settings->m_humidityType) {
+ case RTHUMIDITY_TYPE_HTS221:
+ return new RTHumidityHTS221(settings);
+
+ case RTHUMIDITY_TYPE_HTU21D:
+ return new RTHumidityHTU21D(settings);
+
+ case RTHUMIDITY_TYPE_AUTODISCOVER:
+ if (settings->discoverHumidity(settings->m_humidityType, settings->m_I2CHumidityAddress)) {
+ settings->saveSettings();
+ return RTHumidity::createHumidity(settings);
+ }
+ return NULL;
+
+ case RTHUMIDITY_TYPE_NULL:
+ return NULL;
+
+ default:
+ return NULL;
+ }
+}
+
+
+RTHumidity::RTHumidity(RTIMUSettings *settings)
+{
+ m_settings = settings;
+}
+
+RTHumidity::~RTHumidity()
+{
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.h
new file mode 100644
index 0000000..04e4168
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidity.h
@@ -0,0 +1,55 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTHUMIDITY_H
+#define _RTHUMIDITY_H
+
+#include "RTIMUSettings.h"
+#include "RTIMULibDefs.h"
+#include "RTHumidityDefs.h"
+
+class RTHumidity
+{
+public:
+ // Humidity sensor objects should always be created with the following call
+
+ static RTHumidity *createHumidity(RTIMUSettings *settings);
+
+ // Constructor/destructor
+
+ RTHumidity(RTIMUSettings *settings);
+ virtual ~RTHumidity();
+
+ // These functions must be provided by sub classes
+
+ virtual const char *humidityName() = 0; // the name of the humidity sensor
+ virtual int humidityType() = 0; // the type code of the humidity sensor
+ virtual bool humidityInit() = 0; // set up the humidity sensor
+ virtual bool humidityRead(RTIMU_DATA& data) = 0; // get latest value
+
+protected:
+ RTIMUSettings *m_settings; // the settings object pointer
+
+};
+
+#endif // _RTHUMIDITY_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityDefs.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityDefs.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityDefs.h
new file mode 100644
index 0000000..940b0d0
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityDefs.h
@@ -0,0 +1,82 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTHUMIDITYDEFS_H
+#define _RTHUMIDITYDEFS_H
+
+// Pressure sensor type codes
+
+#define RTHUMIDITY_TYPE_AUTODISCOVER 0 // audodiscover the humidity sensor
+#define RTHUMIDITY_TYPE_NULL 1 // if no physical hardware
+#define RTHUMIDITY_TYPE_HTS221 2 // HTS221
+#define RTHUMIDITY_TYPE_HTU21D 3 // HTU21D
+
+//----------------------------------------------------------
+//
+// HTS221
+
+// HTS221 I2C Slave Address
+
+#define HTS221_ADDRESS 0x5f
+#define HTS221_REG_ID 0x0f
+#define HTS221_ID 0xbc
+
+// Register map
+
+#define HTS221_WHO_AM_I 0x0f
+#define HTS221_AV_CONF 0x10
+#define HTS221_CTRL1 0x20
+#define HTS221_CTRL2 0x21
+#define HTS221_CTRL3 0x22
+#define HTS221_STATUS 0x27
+#define HTS221_HUMIDITY_OUT_L 0x28
+#define HTS221_HUMIDITY_OUT_H 0x29
+#define HTS221_TEMP_OUT_L 0x2a
+#define HTS221_TEMP_OUT_H 0x2b
+#define HTS221_H0_H_2 0x30
+#define HTS221_H1_H_2 0x31
+#define HTS221_T0_C_8 0x32
+#define HTS221_T1_C_8 0x33
+#define HTS221_T1_T0 0x35
+#define HTS221_H0_T0_OUT 0x36
+#define HTS221_H1_T0_OUT 0x3a
+#define HTS221_T0_OUT 0x3c
+#define HTS221_T1_OUT 0x3e
+
+//----------------------------------------------------------
+//
+// HTU21D
+
+// HTU21D I2C Slave Address
+
+#define HTU21D_ADDRESS 0x40
+
+// Register map
+
+#define HTU21D_CMD_TRIG_TEMP 0xf3
+#define HTU21D_CMD_TRIG_HUM 0xf5
+#define HTU21D_WRITE_USER_REG 0xe6
+#define HTU21D_READ_USER_REG 0xe7
+#define HTU21D_CMD_SOFT_RESET 0xfe
+
+#endif // _RTHUMIDITYDEFS_H
[5/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp
new file mode 100644
index 0000000..d8ab502
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp
@@ -0,0 +1,537 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMUGD20M303DLHC.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUGD20M303DLHC::RTIMUGD20M303DLHC(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMUGD20M303DLHC::~RTIMUGD20M303DLHC()
+{
+}
+
+bool RTIMUGD20M303DLHC::IMUInit()
+{
+ unsigned char result;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+ m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
+ m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, 0x80, "Failed to boot L3GD20"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_WHO_AM_I, 1, &result, "Failed to read L3GD20 id"))
+ return false;
+
+ if (result != L3GD20_ID) {
+ HAL_ERROR1("Incorrect L3GD20 id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL4())
+ return false;
+
+ // Set up the compass
+
+ if (!setCompassCRA())
+ return false;
+
+ if (!setCompassCRB())
+ return false;
+
+ if (!setCompassCRM())
+ return false;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("GD20M303DLHC init complete\n");
+ return true;
+}
+
+bool RTIMUGD20M303DLHC::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+
+ switch (m_settings->m_GD20M303DLHCGyroSampleRate) {
+ case L3GD20_SAMPLERATE_95:
+ ctrl1 = 0x0f;
+ m_sampleRate = 95;
+ break;
+
+ case L3GD20_SAMPLERATE_190:
+ ctrl1 = 0x4f;
+ m_sampleRate = 190;
+ break;
+
+ case L3GD20_SAMPLERATE_380:
+ ctrl1 = 0x8f;
+ m_sampleRate = 380;
+ break;
+
+ case L3GD20_SAMPLERATE_760:
+ ctrl1 = 0xcf;
+ m_sampleRate = 760;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20 sample rate code %d\n", m_settings->m_GD20M303DLHCGyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_GD20M303DLHCGyroBW) {
+ case L3GD20_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case L3GD20_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case L3GD20_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case L3GD20_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL1, ctrl1, "Failed to set L3GD20 CTRL1"));
+}
+
+bool RTIMUGD20M303DLHC::setGyroCTRL2()
+{
+ if ((m_settings->m_GD20M303DLHCGyroHpf < L3GD20_HPF_0) || (m_settings->m_GD20M303DLHCGyroHpf > L3GD20_HPF_9)) {
+ HAL_ERROR1("Illegal L3GD20 high pass filter code %d\n", m_settings->m_GD20M303DLHCGyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL2, m_settings->m_GD20M303DLHCGyroHpf, "Failed to set L3GD20 CTRL2");
+}
+
+bool RTIMUGD20M303DLHC::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20M303DLHCGyroFsr) {
+ case L3GD20_FSR_250:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20 FSR code %d\n", m_settings->m_GD20M303DLHCGyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL4, ctrl4, "Failed to set L3GD20 CTRL4");
+}
+
+
+bool RTIMUGD20M303DLHC::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, ctrl5, "Failed to set L3GD20 CTRL5");
+}
+
+
+bool RTIMUGD20M303DLHC::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_GD20M303DLHCAccelSampleRate < 1) || (m_settings->m_GD20M303DLHCAccelSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC accel sample rate code %d\n", m_settings->m_GD20M303DLHCAccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_GD20M303DLHCAccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL1_A, ctrl1, "Failed to set LSM303D CTRL1");
+}
+
+bool RTIMUGD20M303DLHC::setAccelCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20M303DLHCAccelFsr) {
+ case LSM303DLHC_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)16;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC accel FSR code %d\n", m_settings->m_GD20M303DLHCAccelFsr);
+ return false;
+ }
+
+ ctrl4 = 0x80 + (m_settings->m_GD20M303DLHCAccelFsr << 4);
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4");
+}
+
+
+bool RTIMUGD20M303DLHC::setCompassCRA()
+{
+ unsigned char cra;
+
+ if ((m_settings->m_GD20M303DLHCCompassSampleRate < 0) || (m_settings->m_GD20M303DLHCCompassSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC compass sample rate code %d\n", m_settings->m_GD20M303DLHCCompassSampleRate);
+ return false;
+ }
+
+ cra = (m_settings->m_GD20M303DLHCCompassSampleRate << 2);
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRA_M, cra, "Failed to set LSM303DLHC CRA_M");
+}
+
+bool RTIMUGD20M303DLHC::setCompassCRB()
+{
+ unsigned char crb;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_GD20M303DLHCCompassFsr) {
+ case LSM303DLHC_COMPASS_FSR_1_3:
+ crb = 0x20;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_1_9:
+ crb = 0x40;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_2_5:
+ crb = 0x60;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4:
+ crb = 0x80;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4_7:
+ crb = 0xa0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_5_6:
+ crb = 0xc0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_8_1:
+ crb = 0xe0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC compass FSR code %d\n", m_settings->m_GD20M303DLHCCompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRB_M, crb, "Failed to set LSM303DLHC CRB_M");
+}
+
+bool RTIMUGD20M303DLHC::setCompassCRM()
+{
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRM_M, 0x00, "Failed to set LSM303DLHC CRM_M");
+}
+
+
+int RTIMUGD20M303DLHC::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20M303DLHC::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_FIFO_SRC, 1, &status, "Failed to read L3GD20 fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("L3GD20 fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, 0x10, "Failed to set L3GD20 CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < GD20M303DLHC_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= GD20M303DLHC_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == GD20M303DLHC_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == GD20M303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, GD20M303DLHC_FIFO_CHUNK_SIZE * GD20M303DLHC_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read L3GD20 fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = GD20M303DLHC_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == GD20M303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20M303DLHC_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += GD20M303DLHC_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == GD20M303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_STATUS, 1, &status, "Failed to read L3GD20 status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+
+ m_imuData.compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
+ m_imuData.compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
+ m_imuData.compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ RTFLOAT temp;
+
+ temp = m_imuData.compass.z();
+ m_imuData.compass.setZ(-m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h
new file mode 100644
index 0000000..6d2aa9c
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h
@@ -0,0 +1,97 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUGD20M303DLHC_H
+#define _RTIMUGD20M303DLHC_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define GD20M303DLHC_CACHE_MODE // not reliable at the moment
+
+#ifdef GD20M303DLHC_CACHE_MODE
+
+// Cache defs
+
+#define GD20M303DLHC_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define GD20M303DLHC_FIFO_THRESH 16 // threshold point in fifo
+#define GD20M303DLHC_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[GD20M303DLHC_FIFO_THRESH * GD20M303DLHC_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} GD20M303DLHC_CACHE_BLOCK;
+
+#endif
+
+class RTIMUGD20M303DLHC : public RTIMU
+{
+public:
+ RTIMUGD20M303DLHC(RTIMUSettings *settings);
+ ~RTIMUGD20M303DLHC();
+
+ virtual const char *IMUName() { return "L3GD20 + LSM303DLHC"; }
+ virtual int IMUType() { return RTIMU_TYPE_GD20M303DLHC; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL4();
+ bool setCompassCRA();
+ bool setCompassCRB();
+ bool setCompassCRM();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of L3GD20
+ unsigned char m_accelSlaveAddr; // I2C address of LSM303DLHC accel
+ unsigned char m_compassSlaveAddr; // I2C address of LSM303DLHC compass
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScaleXY;
+ RTFLOAT m_compassScaleZ;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ GD20M303DLHC_CACHE_BLOCK m_cache[GD20M303DLHC_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMUGD20M303DLHC_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp
new file mode 100644
index 0000000..676d111
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp
@@ -0,0 +1,538 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMULSM9DS0.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMULSM9DS0::RTIMULSM9DS0(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMULSM9DS0::~RTIMULSM9DS0()
+{
+}
+
+bool RTIMULSM9DS0::IMUInit()
+{
+ unsigned char result;
+
+#ifdef LSM9DS0_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+
+ // work out accelmag address
+
+ if (m_settings->HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM9DS0_ACCELMAG_ID) {
+ m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0;
+ }
+ } else {
+ m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1;
+ }
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id"))
+ return false;
+
+ if (result != LSM9DS0_GYRO_ID) {
+ HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id"))
+ return false;
+
+ if (result != LSM9DS0_ACCELMAG_ID) {
+ HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result);
+ return false;
+ }
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL2())
+ return false;
+
+ if (!setCompassCTRL5())
+ return false;
+
+ if (!setCompassCTRL6())
+ return false;
+
+ if (!setCompassCTRL7())
+ return false;
+
+#ifdef LSM9DS0_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("LSM9DS0 init complete\n");
+ return true;
+}
+
+bool RTIMULSM9DS0::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+
+ switch (m_settings->m_LSM9DS0GyroSampleRate) {
+ case LSM9DS0_GYRO_SAMPLERATE_95:
+ ctrl1 = 0x0f;
+ m_sampleRate = 95;
+ break;
+
+ case LSM9DS0_GYRO_SAMPLERATE_190:
+ ctrl1 = 0x4f;
+ m_sampleRate = 190;
+ break;
+
+ case LSM9DS0_GYRO_SAMPLERATE_380:
+ ctrl1 = 0x8f;
+ m_sampleRate = 380;
+ break;
+
+ case LSM9DS0_GYRO_SAMPLERATE_760:
+ ctrl1 = 0xcf;
+ m_sampleRate = 760;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS0 gyro sample rate code %d\n", m_settings->m_LSM9DS0GyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_LSM9DS0GyroBW) {
+ case LSM9DS0_GYRO_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case LSM9DS0_GYRO_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case LSM9DS0_GYRO_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case LSM9DS0_GYRO_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1, "Failed to set LSM9DS0 gyro CTRL1"));
+}
+
+bool RTIMULSM9DS0::setGyroCTRL2()
+{
+ if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) {
+ HAL_ERROR1("Illegal LSM9DS0 gyro high pass filter code %d\n", m_settings->m_LSM9DS0GyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf, "Failed to set LSM9DS0 gyro CTRL2");
+}
+
+bool RTIMULSM9DS0::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_LSM9DS0GyroFsr) {
+ case LSM9DS0_GYRO_FSR_250:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case LSM9DS0_GYRO_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case LSM9DS0_GYRO_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS0 gyro FSR code %d\n", m_settings->m_LSM9DS0GyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL4, ctrl4, "Failed to set LSM9DS0 gyro CTRL4");
+}
+
+
+bool RTIMULSM9DS0::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef LSM9DS0_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, ctrl5, "Failed to set LSM9DS0 gyro CTRL5");
+}
+
+
+bool RTIMULSM9DS0::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) {
+ HAL_ERROR1("Illegal LSM9DS0 accel sample rate code %d\n", m_settings->m_LSM9DS0AccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL1, ctrl1, "Failed to set LSM9DS0 accell CTRL1");
+}
+
+bool RTIMULSM9DS0::setAccelCTRL2()
+{
+ unsigned char ctrl2;
+
+ if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) {
+ HAL_ERROR1("Illegal LSM9DS0 accel low pass fiter code %d\n", m_settings->m_LSM9DS0AccelLpf);
+ return false;
+ }
+
+ switch (m_settings->m_LSM9DS0AccelFsr) {
+ case LSM9DS0_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.000061;
+ break;
+
+ case LSM9DS0_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.000122;
+ break;
+
+ case LSM9DS0_ACCEL_FSR_6:
+ m_accelScale = (RTFLOAT)0.000183;
+ break;
+
+ case LSM9DS0_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.000244;
+ break;
+
+ case LSM9DS0_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.000732;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS0 accel FSR code %d\n", m_settings->m_LSM9DS0AccelFsr);
+ return false;
+ }
+
+ ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3);
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL2, ctrl2, "Failed to set LSM9DS0 accel CTRL2");
+}
+
+
+bool RTIMULSM9DS0::setCompassCTRL5()
+{
+ unsigned char ctrl5;
+
+ if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) {
+ HAL_ERROR1("Illegal LSM9DS0 compass sample rate code %d\n", m_settings->m_LSM9DS0CompassSampleRate);
+ return false;
+ }
+
+ ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2);
+
+#ifdef LSM9DS0_CACHE_MODE
+ // enable fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL5, ctrl5, "Failed to set LSM9DS0 compass CTRL5");
+}
+
+bool RTIMULSM9DS0::setCompassCTRL6()
+{
+ unsigned char ctrl6;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_LSM9DS0CompassFsr) {
+ case LSM9DS0_COMPASS_FSR_2:
+ ctrl6 = 0;
+ m_compassScale = (RTFLOAT)0.008;
+ break;
+
+ case LSM9DS0_COMPASS_FSR_4:
+ ctrl6 = 0x20;
+ m_compassScale = (RTFLOAT)0.016;
+ break;
+
+ case LSM9DS0_COMPASS_FSR_8:
+ ctrl6 = 0x40;
+ m_compassScale = (RTFLOAT)0.032;
+ break;
+
+ case LSM9DS0_COMPASS_FSR_12:
+ ctrl6 = 0x60;
+ m_compassScale = (RTFLOAT)0.0479;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS0 compass FSR code %d\n", m_settings->m_LSM9DS0CompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL6, ctrl6, "Failed to set LSM9DS0 compass CTRL6");
+}
+
+bool RTIMULSM9DS0::setCompassCTRL7()
+{
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL7, 0x60, "Failed to set LSM9DS0CTRL7");
+}
+
+
+
+int RTIMULSM9DS0::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMULSM9DS0::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef LSM9DS0_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_SRC, 1, &status, "Failed to read LSM9DS0 gyro fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("LSM9DS0 gyro fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x10, "Failed to set LSM9DS0 gyro CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x0, "Failed to set LSM9DS0 gyro FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 gyro FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < LSM9DS0_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= LSM9DS0_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == LSM9DS0_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, LSM9DS0_FIFO_CHUNK_SIZE * LSM9DS0_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read LSM9DS0 fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM9DS0 accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM9DS0 compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = LSM9DS0_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == LSM9DS0_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, LSM9DS0_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += LSM9DS0_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, 1, &status, "Failed to read LSM9DS0 status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+ RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);
+
+ // sort out gyro axes and correct for bias
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ m_imuData.compass.setY(-m_imuData.compass.y());
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h
new file mode 100644
index 0000000..814bd3f
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h
@@ -0,0 +1,95 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMULSM9DS0_H
+#define _RTIMULSM9DS0_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define LSM9DS0_CACHE_MODE // not reliable at the moment
+
+#ifdef LSM9DS0_CACHE_MODE
+
+// Cache defs
+
+#define LSM9DS0_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define LSM9DS0_FIFO_THRESH 16 // threshold point in fifo
+#define LSM9DS0_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[LSM9DS0_FIFO_THRESH * LSM9DS0_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} LSM9DS0_CACHE_BLOCK;
+
+#endif
+
+class RTIMULSM9DS0 : public RTIMU
+{
+public:
+ RTIMULSM9DS0(RTIMUSettings *settings);
+ ~RTIMULSM9DS0();
+
+ virtual const char *IMUName() { return "LSM9DS0"; }
+ virtual int IMUType() { return RTIMU_TYPE_LSM9DS0; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL2();
+ bool setCompassCTRL5();
+ bool setCompassCTRL6();
+ bool setCompassCTRL7();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of gyro
+ unsigned char m_accelCompassSlaveAddr; // I2C address of accel and mag
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScale;
+
+#ifdef LSM9DS0_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ LSM9DS0_CACHE_BLOCK m_cache[LSM9DS0_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMULSM9DS0_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp
new file mode 100644
index 0000000..b84b6b7
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp
@@ -0,0 +1,408 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMULSM9DS1.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMULSM9DS1::RTIMULSM9DS1(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMULSM9DS1::~RTIMULSM9DS1()
+{
+}
+
+bool RTIMULSM9DS1::IMUInit()
+{
+ unsigned char result;
+
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_accelGyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+
+ // work outmag address
+
+ if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM9DS1_MAG_ID) {
+ m_magSlaveAddr = LSM9DS1_MAG_ADDRESS0;
+ }
+ } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM9DS1_MAG_ID) {
+ m_magSlaveAddr = LSM9DS1_MAG_ADDRESS1;
+ }
+ } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM9DS1_MAG_ID) {
+ m_magSlaveAddr = LSM9DS1_MAG_ADDRESS2;
+ }
+ } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM9DS1_MAG_ID) {
+ m_magSlaveAddr = LSM9DS1_MAG_ADDRESS3;
+ }
+ }
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro/accel
+
+ if (!m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL8, 0x80, "Failed to boot LSM9DS1"))
+ return false;
+
+ m_settings->delayMs(100);
+
+ if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/gyro id"))
+ return false;
+
+ if (result != LSM9DS1_ID) {
+ HAL_ERROR1("Incorrect LSM9DS1 gyro id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL3())
+ return false;
+
+ // Set up the mag
+
+ if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/mag id"))
+ return false;
+
+ if (result != LSM9DS1_MAG_ID) {
+ HAL_ERROR1("Incorrect LSM9DS1 accel/mag id %d\n", result);
+ return false;
+ }
+
+ if (!setAccelCTRL6())
+ return false;
+
+ if (!setAccelCTRL7())
+ return false;
+
+ if (!setCompassCTRL1())
+ return false;
+
+ if (!setCompassCTRL2())
+ return false;
+
+ if (!setCompassCTRL3())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("LSM9DS1 init complete\n");
+ return true;
+}
+
+bool RTIMULSM9DS1::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+
+ switch (m_settings->m_LSM9DS1GyroSampleRate) {
+ case LSM9DS1_GYRO_SAMPLERATE_14_9:
+ ctrl1 = 0x20;
+ m_sampleRate = 15;
+ break;
+
+ case LSM9DS1_GYRO_SAMPLERATE_59_5:
+ ctrl1 = 0x40;
+ m_sampleRate = 60;
+ break;
+
+ case LSM9DS1_GYRO_SAMPLERATE_119:
+ ctrl1 = 0x60;
+ m_sampleRate = 119;
+ break;
+
+ case LSM9DS1_GYRO_SAMPLERATE_238:
+ ctrl1 = 0x80;
+ m_sampleRate = 238;
+ break;
+
+ case LSM9DS1_GYRO_SAMPLERATE_476:
+ ctrl1 = 0xa0;
+ m_sampleRate = 476;
+ break;
+
+ case LSM9DS1_GYRO_SAMPLERATE_952:
+ ctrl1 = 0xc0;
+ m_sampleRate = 952;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS1 gyro sample rate code %d\n", m_settings->m_LSM9DS1GyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_LSM9DS1GyroBW) {
+ case LSM9DS1_GYRO_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case LSM9DS1_GYRO_BANDWIDTH_1:
+ ctrl1 |= 0x01;
+ break;
+
+ case LSM9DS1_GYRO_BANDWIDTH_2:
+ ctrl1 |= 0x02;
+ break;
+
+ case LSM9DS1_GYRO_BANDWIDTH_3:
+ ctrl1 |= 0x03;
+ break;
+ }
+
+ switch (m_settings->m_LSM9DS1GyroFsr) {
+ case LSM9DS1_GYRO_FSR_250:
+ ctrl1 |= 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case LSM9DS1_GYRO_FSR_500:
+ ctrl1 |= 0x08;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case LSM9DS1_GYRO_FSR_2000:
+ ctrl1 |= 0x18;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS1 gyro FSR code %d\n", m_settings->m_LSM9DS1GyroFsr);
+ return false;
+ }
+ return (m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL1, ctrl1, "Failed to set LSM9DS1 gyro CTRL1"));
+}
+
+bool RTIMULSM9DS1::setGyroCTRL3()
+{
+ unsigned char ctrl3;
+
+ if ((m_settings->m_LSM9DS1GyroHpf < LSM9DS1_GYRO_HPF_0) || (m_settings->m_LSM9DS1GyroHpf > LSM9DS1_GYRO_HPF_9)) {
+ HAL_ERROR1("Illegal LSM9DS1 gyro high pass filter code %d\n", m_settings->m_LSM9DS1GyroHpf);
+ return false;
+ }
+ ctrl3 = m_settings->m_LSM9DS1GyroHpf;
+
+ // Turn on hpf
+ ctrl3 |= 0x40;
+
+ return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL3, ctrl3, "Failed to set LSM9DS1 gyro CTRL3");
+}
+
+bool RTIMULSM9DS1::setAccelCTRL6()
+{
+ unsigned char ctrl6;
+
+ if ((m_settings->m_LSM9DS1AccelSampleRate < 0) || (m_settings->m_LSM9DS1AccelSampleRate > 6)) {
+ HAL_ERROR1("Illegal LSM9DS1 accel sample rate code %d\n", m_settings->m_LSM9DS1AccelSampleRate);
+ return false;
+ }
+
+ ctrl6 = (m_settings->m_LSM9DS1AccelSampleRate << 5);
+
+ if ((m_settings->m_LSM9DS1AccelLpf < 0) || (m_settings->m_LSM9DS1AccelLpf > 3)) {
+ HAL_ERROR1("Illegal LSM9DS1 accel low pass fiter code %d\n", m_settings->m_LSM9DS1AccelLpf);
+ return false;
+ }
+
+ switch (m_settings->m_LSM9DS1AccelFsr) {
+ case LSM9DS1_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.000061;
+ break;
+
+ case LSM9DS1_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.000122;
+ break;
+
+ case LSM9DS1_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.000244;
+ break;
+
+ case LSM9DS1_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.000732;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS1 accel FSR code %d\n", m_settings->m_LSM9DS1AccelFsr);
+ return false;
+ }
+
+ ctrl6 |= (m_settings->m_LSM9DS1AccelLpf) | (m_settings->m_LSM9DS1AccelFsr << 3);
+
+ return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL6, ctrl6, "Failed to set LSM9DS1 accel CTRL6");
+}
+
+bool RTIMULSM9DS1::setAccelCTRL7()
+{
+ unsigned char ctrl7;
+
+ ctrl7 = 0x00;
+ //Bug: Bad things happen.
+ //ctrl7 = 0x05;
+
+ return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL7, ctrl7, "Failed to set LSM9DS1 accel CTRL7");
+}
+
+
+bool RTIMULSM9DS1::setCompassCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_LSM9DS1CompassSampleRate < 0) || (m_settings->m_LSM9DS1CompassSampleRate > 5)) {
+ HAL_ERROR1("Illegal LSM9DS1 compass sample rate code %d\n", m_settings->m_LSM9DS1CompassSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_LSM9DS1CompassSampleRate << 2);
+
+ return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL1, ctrl1, "Failed to set LSM9DS1 compass CTRL5");
+}
+
+bool RTIMULSM9DS1::setCompassCTRL2()
+{
+ unsigned char ctrl2;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_LSM9DS1CompassFsr) {
+ case LSM9DS1_COMPASS_FSR_4:
+ ctrl2 = 0;
+ m_compassScale = (RTFLOAT)0.014;
+ break;
+
+ case LSM9DS1_COMPASS_FSR_8:
+ ctrl2 = 0x20;
+ m_compassScale = (RTFLOAT)0.029;
+ break;
+
+ case LSM9DS1_COMPASS_FSR_12:
+ ctrl2 = 0x40;
+ m_compassScale = (RTFLOAT)0.043;
+ break;
+
+ case LSM9DS1_COMPASS_FSR_16:
+ ctrl2 = 0x60;
+ m_compassScale = (RTFLOAT)0.058;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM9DS1 compass FSR code %d\n", m_settings->m_LSM9DS1CompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL2, ctrl2, "Failed to set LSM9DS1 compass CTRL6");
+}
+
+bool RTIMULSM9DS1::setCompassCTRL3()
+{
+ return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL3, 0x00, "Failed to set LSM9DS1 compass CTRL3");
+}
+
+int RTIMULSM9DS1::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMULSM9DS1::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+ if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_STATUS, 1, &status, "Failed to read LSM9DS1 status"))
+ return false;
+
+ if ((status & 0x3) == 0)
+ return false;
+
+ for (int i = 0; i<6; i++){
+ if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_G + i, 1, &gyroData[i], "Failed to read LSM9DS1 gyro data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_XL + i, 1, &accelData[i], "Failed to read LSM9DS1 accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_OUT_X_L + i, 1, &compassData[i], "Failed to read LSM9DS1 compass data"))
+ return false;
+ }
+
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+ RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);
+
+ // sort out gyro axes and correct for bias
+
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+ m_imuData.accel.setY(-m_imuData.accel.y());
+
+ // sort out compass axes
+
+ m_imuData.compass.setX(-m_imuData.compass.x());
+ m_imuData.compass.setZ(-m_imuData.compass.z());
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h
new file mode 100644
index 0000000..7f4feda
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h
@@ -0,0 +1,93 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMULSM9DS1_H
+#define _RTIMULSM9DS1_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define LSM9DS1_CACHE_MODE // not reliable at the moment
+
+#ifdef LSM9DS1_CACHE_MODE
+
+// Cache defs
+
+#define LSM9DS1_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define LSM9DS1_FIFO_THRESH 16 // threshold point in fifo
+#define LSM9DS1_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[LSM9DS1_FIFO_THRESH * LSM9DS1_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} LSM9DS1_CACHE_BLOCK;
+
+#endif
+
+class RTIMULSM9DS1 : public RTIMU
+{
+public:
+ RTIMULSM9DS1(RTIMUSettings *settings);
+ ~RTIMULSM9DS1();
+
+ virtual const char *IMUName() { return "LSM9DS1"; }
+ virtual int IMUType() { return RTIMU_TYPE_LSM9DS1; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL3();
+ bool setAccelCTRL6();
+ bool setAccelCTRL7();
+ bool setCompassCTRL1();
+ bool setCompassCTRL2();
+ bool setCompassCTRL3();
+
+ unsigned char m_accelGyroSlaveAddr; // I2C address of accel andgyro
+ unsigned char m_magSlaveAddr; // I2C address of mag
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScale;
+
+#ifdef LSM9DS1_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ LSM9DS1_CACHE_BLOCK m_cache[LSM9DS1_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMULSM9DS1_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp
new file mode 100644
index 0000000..59d3f31
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp
@@ -0,0 +1,633 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMUMPU9150.h"
+#include "RTIMUSettings.h"
+
+RTIMUMPU9150::RTIMUMPU9150(RTIMUSettings *settings) : RTIMU(settings)
+{
+
+}
+
+RTIMUMPU9150::~RTIMUMPU9150()
+{
+}
+
+bool RTIMUMPU9150::setLpf(unsigned char lpf)
+{
+ switch (lpf) {
+ case MPU9150_LPF_256:
+ case MPU9150_LPF_188:
+ case MPU9150_LPF_98:
+ case MPU9150_LPF_42:
+ case MPU9150_LPF_20:
+ case MPU9150_LPF_10:
+ case MPU9150_LPF_5:
+ m_lpf = lpf;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9150 lpf %d\n", lpf);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9150::setSampleRate(int rate)
+{
+ if ((rate < MPU9150_SAMPLERATE_MIN) || (rate > MPU9150_SAMPLERATE_MAX)) {
+ HAL_ERROR1("Illegal sample rate %d\n", rate);
+ return false;
+ }
+ m_sampleRate = rate;
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+ return true;
+}
+
+bool RTIMUMPU9150::setCompassRate(int rate)
+{
+ if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) {
+ HAL_ERROR1("Illegal compass rate %d\n", rate);
+ return false;
+ }
+ m_compassRate = rate;
+ return true;
+}
+
+bool RTIMUMPU9150::setGyroFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9150_GYROFSR_250:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (131.0 * 180.0);
+ return true;
+
+ case MPU9150_GYROFSR_500:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (62.5 * 180.0);
+ return true;
+
+ case MPU9150_GYROFSR_1000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (32.8 * 180.0);
+ return true;
+
+ case MPU9150_GYROFSR_2000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (16.4 * 180.0);
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9150 gyro fsr %d\n", fsr);
+ return false;
+ }
+}
+
+bool RTIMUMPU9150::setAccelFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9150_ACCELFSR_2:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/16384.0;
+ return true;
+
+ case MPU9150_ACCELFSR_4:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/8192.0;
+ return true;
+
+ case MPU9150_ACCELFSR_8:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/4096.0;
+ return true;
+
+ case MPU9150_ACCELFSR_16:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/2048.0;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9150 accel fsr %d\n", fsr);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9150::IMUInit()
+{
+ unsigned char result;
+
+ m_firstTime = true;
+
+#ifdef MPU9150_CACHE_MODE
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_slaveAddr = m_settings->m_I2CSlaveAddress;
+
+ setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate);
+ setCompassRate(m_settings->m_MPU9150CompassSampleRate);
+ setLpf(m_settings->m_MPU9150GyroAccelLpf);
+ setGyroFsr(m_settings->m_MPU9150GyroFsr);
+ setAccelFsr(m_settings->m_MPU9150AccelFsr);
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // reset the MPU9150
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset"))
+ return false;
+
+ m_settings->delayMs(100);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id"))
+ return false;
+
+ if (result != MPU9150_ID) {
+ HAL_ERROR1("Incorrect MPU9150 id %d\n", result);
+ return false;
+ }
+
+ // now configure the various components
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf"))
+ return false;
+
+ if (!setSampleRate())
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr"))
+ return false;
+
+ // now configure compass
+
+ if (!configureCompass())
+ return false;
+
+ // enable the sensors
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
+ return false;
+
+ // select the data to go into the FIFO and enable
+
+ if (!resetFifo())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("MPU9150 init complete\n");
+ return true;
+}
+
+bool RTIMUMPU9150::configureCompass()
+{
+ unsigned char asa[3];
+ unsigned char id;
+
+ m_compassIs5883 = false;
+ m_compassDataLength = 8;
+
+ bypassOn();
+
+ // get fuse ROM data
+
+ if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "")) {
+
+ // check to see if an HMC5883L is fitted
+
+ if (!m_settings->HALRead(HMC5883_ADDRESS, HMC5883_ID, 1, &id, "Failed to find 5883")) {
+ bypassOff();
+
+ // this is returning true so that MPU-6050 by itself will work
+
+ HAL_INFO("Detected MPU-6050 without compass\n");
+
+ m_imuData.compassValid = false;
+ return true;
+ }
+ if (id != 0x48) { // incorrect id for HMC5883L
+
+ bypassOff();
+
+ // this is returning true so that MPU-6050 by itself will work
+
+ HAL_INFO("Detected MPU-6050 without compass\n");
+
+ m_imuData.compassValid = false;
+ return true;
+ }
+
+ // HMC5883 is present - use that
+
+ if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_A, 0x38, "Failed to set HMC5883 config A")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_B, 0x20, "Failed to set HMC5883 config B")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_MODE, 0x00, "Failed to set HMC5883 mode")) {
+ bypassOff();
+ return false;
+ }
+
+ HAL_INFO("Detected MPU-6050 with HMC5883\n");
+
+ m_compassDataLength = 6;
+ m_compassIs5883 = true;
+ } else {
+
+ // convert asa to usable scale factor
+
+ m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
+ m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
+ m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
+
+ if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) {
+ bypassOff();
+ return false;
+ }
+ }
+
+ bypassOff();
+
+ // now set up MPU9150 to talk to the compass chip
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
+ return false;
+
+ if (m_compassIs5883) {
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | HMC5883_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, HMC5883_DATA_X_HI, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x86, "Failed to set slave 0 ctrl"))
+ return false;
+ } else {
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
+ return false;
+
+ }
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc"))
+ return false;
+
+ if (!setCompassRate())
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9150::resetFifo()
+{
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_ENABLE, 0, "Writing int enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_FIFO_EN, 0, "Writing fifo enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0, "Writing user control"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0x04, "Resetting fifo"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0x60, "Enabling the fifo"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_ENABLE, 1, "Writing int enable"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_FIFO_EN, 0x78, "Failed to set FIFO enables"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9150::bypassOn()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl &= ~0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x82, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+
+bool RTIMUMPU9150::bypassOff()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl |= 0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x80, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+bool RTIMUMPU9150::setSampleRate()
+{
+ int clockRate = 1000;
+
+ if (m_lpf == MPU9150_LPF_256)
+ clockRate = 8000;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_SMPRT_DIV, (unsigned char)(clockRate / m_sampleRate - 1),
+ "Failed to set sample rate"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9150::setCompassRate()
+{
+ int rate;
+
+ rate = m_sampleRate / m_compassRate - 1;
+
+ if (rate > 31)
+ rate = 31;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV4_CTRL, rate, "Failed to set slave ctrl 4"))
+ return false;
+ return true;
+}
+
+int RTIMUMPU9150::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUMPU9150::IMURead()
+{
+ unsigned char fifoCount[2];
+ unsigned int count;
+ unsigned char fifoData[12];
+ unsigned char compassData[8];
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
+ return false;
+
+ count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
+
+ if (count == 1024) {
+ HAL_INFO("MPU9150 fifo has overflowed");
+ resetFifo();
+ m_imuData.timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
+ return false;
+ }
+
+
+#ifdef MPU9150_CACHE_MODE
+ if ((m_cacheCount == 0) && (count >= MPU9150_FIFO_CHUNK_SIZE) && (count < (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE))) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
+ return false;
+ } else {
+ if (count >= (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE)) {
+ if (m_cacheCount == MPU9150_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ int blockCount = count / MPU9150_FIFO_CHUNK_SIZE; // number of chunks in fifo
+
+ if (blockCount > MPU9150_CACHE_SIZE)
+ blockCount = MPU9150_CACHE_SIZE;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE * blockCount,
+ m_cache[m_cacheIn].data, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = blockCount;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == MPU9150_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9150_FIFO_CHUNK_SIZE);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 8);
+
+ m_cache[m_cacheOut].index += MPU9150_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ }
+
+#else
+
+ if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
+ // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
+ while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+ count -= MPU9150_FIFO_CHUNK_SIZE;
+ m_imuData.timestamp += m_sampleInterval;
+ }
+ }
+
+ if (count < MPU9150_FIFO_CHUNK_SIZE)
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
+ RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);
+
+ if (m_compassIs5883)
+ RTMath::convertToVector(compassData, m_imuData.compass, 0.092f, true);
+ else
+ RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.3f, false);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+
+ if (m_compassIs5883) {
+ // sort out compass axes
+
+ float temp;
+
+ temp = m_imuData.compass.y();
+ m_imuData.compass.setY(-m_imuData.compass.z());
+ m_imuData.compass.setZ(-temp);
+
+ } else {
+
+ // use the compass fuse data adjustments
+
+ m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
+ m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
+ m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);
+
+ // sort out compass axes
+
+ float temp;
+
+ temp = m_imuData.compass.x();
+ m_imuData.compass.setX(m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+ }
+
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h
new file mode 100644
index 0000000..396a632
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h
@@ -0,0 +1,110 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUMPU9150_H
+#define _RTIMUMPU9150_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+#define MPU9150_CACHE_MODE
+
+// FIFO transfer size
+
+#define MPU9150_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes
+
+#ifdef MPU9150_CACHE_MODE
+
+// Cache mode defines
+
+#define MPU9150_CACHE_SIZE 16 // number of chunks in a block
+#define MPU9150_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[MPU9150_FIFO_CHUNK_SIZE * MPU9150_CACHE_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char compass[8]; // the raw compass readings for the block
+
+} MPU9150_CACHE_BLOCK;
+
+#endif
+
+
+class RTIMUMPU9150 : public RTIMU
+{
+public:
+ RTIMUMPU9150(RTIMUSettings *settings);
+ ~RTIMUMPU9150();
+
+ bool setLpf(unsigned char lpf);
+ bool setSampleRate(int rate);
+ bool setCompassRate(int rate);
+ bool setGyroFsr(unsigned char fsr);
+ bool setAccelFsr(unsigned char fsr);
+
+ virtual const char *IMUName() { return "MPU-9150"; }
+ virtual int IMUType() { return RTIMU_TYPE_MPU9150; }
+ virtual bool IMUInit();
+ virtual bool IMURead();
+ virtual int IMUGetPollInterval();
+
+private:
+ bool configureCompass(); // configures the compass
+ bool bypassOn(); // talk to compass
+ bool bypassOff(); // talk to MPU9150
+ bool setSampleRate();
+ bool setCompassRate();
+ bool resetFifo();
+
+ bool m_firstTime; // if first sample
+
+ unsigned char m_slaveAddr; // I2C address of MPU9150
+
+ unsigned char m_lpf; // low pass filter setting
+ int m_compassRate; // compass sample rate in Hz
+ unsigned char m_gyroFsr;
+ unsigned char m_accelFsr;
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+
+ bool m_compassIs5883; // if it is an MPU-6050/HMC5883 combo
+ int m_compassDataLength; // 8 for MPU-9150, 6 for HMC5883
+ RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use
+
+#ifdef MPU9150_CACHE_MODE
+
+ MPU9150_CACHE_BLOCK m_cache[MPU9150_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+
+};
+
+#endif // _RTIMUMPU9150_H
[4/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp
new file mode 100644
index 0000000..dc6cc35
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp
@@ -0,0 +1,657 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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
+// staslock@gmail.com (www.clickdrive.io)
+
+#include "RTIMUMPU9250.h"
+#include "RTIMUSettings.h"
+
+RTIMUMPU9250::RTIMUMPU9250(RTIMUSettings *settings) : RTIMU(settings)
+{
+
+}
+
+RTIMUMPU9250::~RTIMUMPU9250()
+{
+}
+
+bool RTIMUMPU9250::setSampleRate(int rate)
+{
+ if ((rate < MPU9250_SAMPLERATE_MIN) || (rate > MPU9250_SAMPLERATE_MAX)) {
+ HAL_ERROR1("Illegal sample rate %d\n", rate);
+ return false;
+ }
+
+ // Note: rates interact with the lpf settings
+
+ if ((rate < MPU9250_SAMPLERATE_MAX) && (rate >= 8000))
+ rate = 8000;
+
+ if ((rate < 8000) && (rate >= 1000))
+ rate = 1000;
+
+ if (rate < 1000) {
+ int sampleDiv = (1000 / rate) - 1;
+ m_sampleRate = 1000 / (1 + sampleDiv);
+ } else {
+ m_sampleRate = rate;
+ }
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroLpf(unsigned char lpf)
+{
+ switch (lpf) {
+ case MPU9250_GYRO_LPF_8800:
+ case MPU9250_GYRO_LPF_3600:
+ case MPU9250_GYRO_LPF_250:
+ case MPU9250_GYRO_LPF_184:
+ case MPU9250_GYRO_LPF_92:
+ case MPU9250_GYRO_LPF_41:
+ case MPU9250_GYRO_LPF_20:
+ case MPU9250_GYRO_LPF_10:
+ case MPU9250_GYRO_LPF_5:
+ m_gyroLpf = lpf;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 gyro lpf %d\n", lpf);
+ return false;
+ }
+}
+
+bool RTIMUMPU9250::setAccelLpf(unsigned char lpf)
+{
+ switch (lpf) {
+ case MPU9250_ACCEL_LPF_1130:
+ case MPU9250_ACCEL_LPF_460:
+ case MPU9250_ACCEL_LPF_184:
+ case MPU9250_ACCEL_LPF_92:
+ case MPU9250_ACCEL_LPF_41:
+ case MPU9250_ACCEL_LPF_20:
+ case MPU9250_ACCEL_LPF_10:
+ case MPU9250_ACCEL_LPF_5:
+ m_accelLpf = lpf;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 accel lpf %d\n", lpf);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9250::setCompassRate(int rate)
+{
+ if ((rate < MPU9250_COMPASSRATE_MIN) || (rate > MPU9250_COMPASSRATE_MAX)) {
+ HAL_ERROR1("Illegal compass rate %d\n", rate);
+ return false;
+ }
+ m_compassRate = rate;
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9250_GYROFSR_250:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (131.0 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_500:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (62.5 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_1000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (32.8 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_2000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (16.4 * 180.0);
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 gyro fsr %d\n", fsr);
+ return false;
+ }
+}
+
+bool RTIMUMPU9250::setAccelFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9250_ACCELFSR_2:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/16384.0;
+ return true;
+
+ case MPU9250_ACCELFSR_4:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/8192.0;
+ return true;
+
+ case MPU9250_ACCELFSR_8:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/4096.0;
+ return true;
+
+ case MPU9250_ACCELFSR_16:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/2048.0;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 accel fsr %d\n", fsr);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9250::IMUInit()
+{
+ unsigned char result;
+
+ m_firstTime = true;
+
+#ifdef MPU9250_CACHE_MODE
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_slaveAddr = m_settings->m_I2CSlaveAddress;
+
+ setSampleRate(m_settings->m_MPU9250GyroAccelSampleRate);
+ setCompassRate(m_settings->m_MPU9250CompassSampleRate);
+ setGyroLpf(m_settings->m_MPU9250GyroLpf);
+ setAccelLpf(m_settings->m_MPU9250AccelLpf);
+ setGyroFsr(m_settings->m_MPU9250GyroFsr);
+ setAccelFsr(m_settings->m_MPU9250AccelFsr);
+
+ setCalibrationData();
+
+
+ // enable the bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // reset the MPU9250
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80, "Failed to initiate MPU9250 reset"))
+ return false;
+
+ m_settings->delayMs(100);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00, "Failed to stop MPU9250 reset"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_WHO_AM_I, 1, &result, "Failed to read MPU9250 id"))
+ return false;
+
+ if (result != MPU9250_ID) {
+ HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result);
+ return false;
+ }
+
+ // now configure the various components
+
+ if (!setGyroConfig())
+ return false;
+
+ if (!setAccelConfig())
+ return false;
+
+ if (!setSampleRate())
+ return false;
+
+ if(!compassSetup()) {
+ return false;
+ }
+
+ if (!setCompassRate())
+ return false;
+
+ // enable the sensors
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
+ return false;
+
+ // select the data to go into the FIFO and enable
+
+ if (!resetFifo())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO1("%s init complete\n", IMUName());
+ return true;
+}
+
+
+bool RTIMUMPU9250::resetFifo()
+{
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_ENABLE, 0, "Writing int enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_FIFO_EN, 0, "Writing fifo enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0, "Writing user control"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0x04, "Resetting fifo"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0x60, "Enabling the fifo"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_ENABLE, 1, "Writing int enable"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_FIFO_EN, 0x78, "Failed to set FIFO enables"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroConfig()
+{
+ unsigned char gyroConfig = m_gyroFsr + ((m_gyroLpf >> 3) & 3);
+ unsigned char gyroLpf = m_gyroLpf & 7;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_GYRO_CONFIG, gyroConfig, "Failed to write gyro config"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_GYRO_LPF, gyroLpf, "Failed to write gyro lpf"))
+ return false;
+ return true;
+}
+
+bool RTIMUMPU9250::setAccelConfig()
+{
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_ACCEL_CONFIG, m_accelFsr, "Failed to write accel config"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_ACCEL_LPF, m_accelLpf, "Failed to write accel lpf"))
+ return false;
+ return true;
+}
+
+bool RTIMUMPU9250::setSampleRate()
+{
+ if (m_sampleRate > 1000)
+ return true; // SMPRT not used above 1000Hz
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_SMPRT_DIV, (unsigned char) (1000 / m_sampleRate - 1),
+ "Failed to set sample rate"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::compassSetup() {
+ unsigned char asa[3];
+
+ if (m_settings->m_busIsI2C) {
+ // I2C mode
+
+ bypassOn();
+
+ // get fuse ROM data
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 1")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALRead(AK8963_ADDRESS, AK8963_ASAX, 3, asa, "Failed to read compass fuse ROM")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 2")) {
+ bypassOff();
+ return false;
+ }
+
+ bypassOff();
+
+ } else {
+ // SPI mode
+
+ bypassOff();
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ASAX, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x83, "Failed to set slave 0 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x00, "Failed to set compass in power down mode 2"))
+ return false;
+
+ m_settings->delayMs(10);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x0f, "Failed to set compass in fuse mode"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 3, asa, "Failed to read compass rom"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x0, "Failed to set compass in power down mode 2"))
+ return false;
+ }
+ // both interfaces
+
+ // convert asa to usable scale factor
+
+ m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
+ m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
+ m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::setCompassRate()
+{
+ int rate;
+
+ rate = m_sampleRate / m_compassRate - 1;
+
+ if (rate > 31)
+ rate = 31;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV4_CTRL, rate, "Failed to set slave ctrl 4"))
+ return false;
+ return true;
+}
+
+
+bool RTIMUMPU9250::bypassOn()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl &= ~0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x82, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+
+bool RTIMUMPU9250::bypassOff()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl |= 0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x80, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+
+int RTIMUMPU9250::IMUGetPollInterval()
+{
+ if (m_sampleRate > 400)
+ return 1;
+ else
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUMPU9250::IMURead()
+{
+ unsigned char fifoCount[2];
+ unsigned int count;
+ unsigned char fifoData[12];
+ unsigned char compassData[8];
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
+ return false;
+
+ count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
+
+ if (count == 512) {
+ HAL_INFO("MPU-9250 fifo has overflowed");
+ resetFifo();
+ m_imuData.timestamp += m_sampleInterval * (512 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
+ return false;
+ }
+
+#ifdef MPU9250_CACHE_MODE
+ if ((m_cacheCount == 0) && (count >= MPU9250_FIFO_CHUNK_SIZE) && (count < (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE))) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
+ return false;
+ } else {
+ if (count >= (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE)) {
+ if (m_cacheCount == MPU9250_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ int blockCount = count / MPU9250_FIFO_CHUNK_SIZE; // number of chunks in fifo
+
+ if (blockCount > MPU9250_CACHE_SIZE)
+ blockCount = MPU9250_CACHE_SIZE;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE * blockCount,
+ m_cache[m_cacheIn].data, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = blockCount;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == MPU9250_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9250_FIFO_CHUNK_SIZE);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 8);
+
+ m_cache[m_cacheOut].index += MPU9250_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ }
+
+#else
+
+ if (count > MPU9250_FIFO_CHUNK_SIZE * 40) {
+ // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
+ while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) {
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+ count -= MPU9250_FIFO_CHUNK_SIZE;
+ m_imuData.timestamp += m_sampleInterval;
+ }
+ }
+
+ if (count < MPU9250_FIFO_CHUNK_SIZE)
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
+ RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);
+ RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // use the compass fuse data adjustments
+
+ m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
+ m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
+ m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);
+
+ // sort out compass axes
+
+ float temp;
+
+ temp = m_imuData.compass.x();
+ m_imuData.compass.setX(m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
+
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h
new file mode 100644
index 0000000..e468e28
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h
@@ -0,0 +1,118 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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
+// staslock@gmail.com (www.clickdrive.io)
+
+
+#ifndef _RTIMUMPU9250_H
+#define _RTIMUMPU9250_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+#define MPU9250_CACHE_MODE
+
+// FIFO transfer size
+
+#define MPU9250_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes
+
+#ifdef MPU9250_CACHE_MODE
+
+// Cache mode defines
+
+#define MPU9250_CACHE_SIZE 16 // number of chunks in a block
+#define MPU9250_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[MPU9250_FIFO_CHUNK_SIZE * MPU9250_CACHE_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char compass[8]; // the raw compass readings for the block
+
+} MPU9250_CACHE_BLOCK;
+
+#endif
+
+
+class RTIMUMPU9250 : public RTIMU
+{
+public:
+ RTIMUMPU9250(RTIMUSettings *settings);
+ ~RTIMUMPU9250();
+
+ bool setGyroLpf(unsigned char lpf);
+ bool setAccelLpf(unsigned char lpf);
+ bool setSampleRate(int rate);
+ bool setCompassRate(int rate);
+ bool setGyroFsr(unsigned char fsr);
+ bool setAccelFsr(unsigned char fsr);
+
+ virtual const char *IMUName() { return "MPU-9250"; }
+ virtual int IMUType() { return RTIMU_TYPE_MPU9250; }
+ virtual bool IMUInit();
+ virtual bool IMURead();
+ virtual int IMUGetPollInterval();
+
+protected:
+
+ RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use
+
+private:
+ bool setGyroConfig();
+ bool setAccelConfig();
+ bool setSampleRate();
+ bool compassSetup();
+ bool setCompassRate();
+ bool resetFifo();
+ bool bypassOn();
+ bool bypassOff();
+
+ bool m_firstTime; // if first sample
+
+ unsigned char m_slaveAddr; // I2C address of MPU9150
+
+ unsigned char m_gyroLpf; // gyro low pass filter setting
+ unsigned char m_accelLpf; // accel low pass filter setting
+ int m_compassRate; // compass sample rate in Hz
+ unsigned char m_gyroFsr;
+ unsigned char m_accelFsr;
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+
+
+#ifdef MPU9250_CACHE_MODE
+
+ MPU9250_CACHE_BLOCK m_cache[MPU9250_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+
+};
+
+#endif // _RTIMUMPU9250_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
new file mode 100644
index 0000000..b497dc5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
@@ -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.
+
+#include "RTIMUNull.h"
+#include "RTIMUSettings.h"
+
+RTIMUNull::RTIMUNull(RTIMUSettings *settings) : RTIMU(settings)
+{
+}
+
+RTIMUNull::~RTIMUNull()
+{
+}
+
+bool RTIMUNull::IMUInit()
+{
+ return true;
+}
+
+int RTIMUNull::IMUGetPollInterval()
+{
+ return (100); // just a dummy value really
+}
+
+bool RTIMUNull::IMURead()
+{
+ updateFusion();
+ return true;
+}
+
+void RTIMUNull::setIMUData(const RTIMU_DATA& data)
+{
+ m_imuData = data;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
new file mode 100644
index 0000000..45375d0
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
@@ -0,0 +1,58 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUNULL_H
+#define _RTIMUNULL_H
+
+// IMUNull is a dummy IMU that assumes sensor data is coming from elsewhere,
+// for example, across a network.
+//
+// Call IMUInit in the normal way. Then for every update, call setIMUData and then IMURead
+// to kick the kalman filter.
+
+#include "RTIMU.h"
+
+class RTIMUSettings;
+
+class RTIMUNull : public RTIMU
+{
+public:
+ RTIMUNull(RTIMUSettings *settings);
+ ~RTIMUNull();
+
+ // The timestamp parameter is assumed to be from RTMath::currentUSecsSinceEpoch()
+
+ void setIMUData(const RTIMU_DATA& data);
+
+ virtual const char *IMUName() { return "Null IMU"; }
+ virtual int IMUType() { return RTIMU_TYPE_NULL; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+ virtual bool IMUGyroBiasValid() { return true; }
+
+private:
+ uint64_t m_timestamp;
+};
+
+#endif // _RTIMUNULL_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
new file mode 100644
index 0000000..6074386
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
@@ -0,0 +1,70 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressure.h"
+
+#include "RTPressureBMP180.h"
+#include "RTPressureLPS25H.h"
+#include "RTPressureMS5611.h"
+#include "RTPressureMS5637.h"
+
+RTPressure *RTPressure::createPressure(RTIMUSettings *settings)
+{
+ switch (settings->m_pressureType) {
+ case RTPRESSURE_TYPE_BMP180:
+ return new RTPressureBMP180(settings);
+
+ case RTPRESSURE_TYPE_LPS25H:
+ return new RTPressureLPS25H(settings);
+
+ case RTPRESSURE_TYPE_MS5611:
+ return new RTPressureMS5611(settings);
+
+ case RTPRESSURE_TYPE_MS5637:
+ return new RTPressureMS5637(settings);
+
+ case RTPRESSURE_TYPE_AUTODISCOVER:
+ if (settings->discoverPressure(settings->m_pressureType, settings->m_I2CPressureAddress)) {
+ settings->saveSettings();
+ return RTPressure::createPressure(settings);
+ }
+ return NULL;
+
+ case RTPRESSURE_TYPE_NULL:
+ return NULL;
+
+ default:
+ return NULL;
+ }
+}
+
+
+RTPressure::RTPressure(RTIMUSettings *settings)
+{
+ m_settings = settings;
+}
+
+RTPressure::~RTPressure()
+{
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
new file mode 100644
index 0000000..aa95553
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
@@ -0,0 +1,55 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSURE_H
+#define _RTPRESSURE_H
+
+#include "RTIMUSettings.h"
+#include "RTIMULibDefs.h"
+#include "RTPressureDefs.h"
+
+class RTPressure
+{
+public:
+ // Pressure sensor objects should always be created with the following call
+
+ static RTPressure *createPressure(RTIMUSettings *settings);
+
+ // Constructor/destructor
+
+ RTPressure(RTIMUSettings *settings);
+ virtual ~RTPressure();
+
+ // These functions must be provided by sub classes
+
+ virtual const char *pressureName() = 0; // the name of the pressure sensor
+ virtual int pressureType() = 0; // the type code of the pressure sensor
+ virtual bool pressureInit() = 0; // set up the pressure sensor
+ virtual bool pressureRead(RTIMU_DATA& data) = 0; // get latest value
+
+protected:
+ RTIMUSettings *m_settings; // the settings object pointer
+
+};
+
+#endif // _RTPRESSURE_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
new file mode 100644
index 0000000..e2a5a87
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
@@ -0,0 +1,230 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureBMP180.h"
+
+RTPressureBMP180::RTPressureBMP180(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureBMP180::~RTPressureBMP180()
+{
+}
+
+bool RTPressureBMP180::pressureInit()
+{
+ unsigned char result;
+ unsigned char data[22];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // check ID of chip
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_ID, 1, &result, "Failed to read BMP180 id"))
+ return false;
+
+ if (result != BMP180_ID) {
+ HAL_ERROR1("Incorrect BMP180 id %d\n", result);
+ return false;
+ }
+
+ // get calibration data
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_AC1, 22, data, "Failed to read BMP180 calibration data"))
+ return false;
+
+ m_AC1 = (int16_t)(((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+ m_AC2 = (int16_t)(((uint16_t)data[2]) << 8) + (uint16_t)data[3];
+ m_AC3 = (int16_t)(((uint16_t)data[4]) << 8) + (uint16_t)data[4];
+ m_AC4 = (((uint16_t)data[6]) << 8) + (uint16_t)data[7];
+ m_AC5 = (((uint16_t)data[8]) << 8) + (uint16_t)data[9];
+ m_AC6 = (((uint16_t)data[10]) << 8) + (uint16_t)data[11];
+ m_B1 = (int16_t)(((uint16_t)data[12]) << 8) + (uint16_t)data[13];
+ m_B2 = (int16_t)(((uint16_t)data[14]) << 8) + (uint16_t)data[15];
+ m_MB = (int16_t)(((uint16_t)data[16]) << 8) + (uint16_t)data[17];
+ m_MC = (int16_t)(((uint16_t)data[18]) << 8) + (uint16_t)data[19];
+ m_MD = (int16_t)(((uint16_t)data[20]) << 8) + (uint16_t)data[21];
+
+ m_state = BMP180_STATE_IDLE;
+ m_oss = BMP180_SCO_PRESSURECONV_ULP;
+ return true;
+}
+
+bool RTPressureBMP180::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == BMP180_STATE_IDLE) {
+ // start a temperature conversion
+ if (!m_settings->HALWrite(m_pressureAddr, BMP180_REG_SCO, BMP180_SCO_TEMPCONV, "Failed to start temperature conversion")) {
+ return false;
+ } else {
+ m_state = BMP180_STATE_TEMPERATURE;
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ // printf("P: %f, T: %f\n", m_pressure, m_temperature);
+ }
+ return true;
+}
+
+
+void RTPressureBMP180::pressureBackground()
+{
+ uint8_t data[2];
+
+ switch (m_state) {
+ case BMP180_STATE_IDLE:
+ break;
+
+ case BMP180_STATE_TEMPERATURE:
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to read BMP180 temp conv status")) {
+ break;
+ }
+ if ((data[0] & 0x20) == 0x20)
+ break; // conversion not finished
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_RESULT, 2, data, "Failed to read BMP180 temp conv result")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_rawTemperature = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+
+ data[0] = 0x34 + (m_oss << 6);
+ if (!m_settings->HALWrite(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to start pressure conversion")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_state = BMP180_STATE_PRESSURE;
+ break;
+
+ case BMP180_STATE_PRESSURE:
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to read BMP180 pressure conv status")) {
+ break;
+ }
+ if ((data[0] & 0x20) == 0x20)
+ break; // conversion not finished
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_RESULT, 2, data, "Failed to read BMP180 temp conv result")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_rawPressure = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_XLSB, 1, data, "Failed to read BMP180 XLSB")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+
+ // call this function for testing only
+ // should give T = 150 (15.0C) and pressure 6996 (699.6hPa)
+
+ // setTestData();
+
+ int32_t pressure = ((((uint32_t)(m_rawPressure)) << 8) + (uint32_t)(data[0])) >> (8 - m_oss);
+
+ m_state = BMP180_STATE_IDLE;
+
+ // calculate compensated temperature
+
+ int32_t X1 = (((int32_t)m_rawTemperature - m_AC6) * m_AC5) / 32768;
+
+ if ((X1 + m_MD) == 0) {
+ break;
+ }
+
+ int32_t X2 = (m_MC * 2048) / (X1 + m_MD);
+ int32_t B5 = X1 + X2;
+ m_temperature = (RTFLOAT)((B5 + 8) / 16) / (RTFLOAT)10;
+
+ // calculate compensated pressure
+
+ int32_t B6 = B5 - 4000;
+ // printf("B6 = %d\n", B6);
+ X1 = (m_B2 * ((B6 * B6) / 4096)) / 2048;
+ // printf("X1 = %d\n", X1);
+ X2 = (m_AC2 * B6) / 2048;
+ // printf("X2 = %d\n", X2);
+ int32_t X3 = X1 + X2;
+ // printf("X3 = %d\n", X3);
+ int32_t B3 = (((m_AC1 * 4 + X3) << m_oss) + 2) / 4;
+ // printf("B3 = %d\n", B3);
+ X1 = (m_AC3 * B6) / 8192;
+ // printf("X1 = %d\n", X1);
+ X2 = (m_B1 * ((B6 * B6) / 4096)) / 65536;
+ // printf("X2 = %d\n", X2);
+ X3 = ((X1 + X2) + 2) / 4;
+ // printf("X3 = %d\n", X3);
+ int32_t B4 = (m_AC4 * (unsigned long)(X3 + 32768)) / 32768;
+ // printf("B4 = %d\n", B4);
+ uint32_t B7 = ((unsigned long)pressure - B3) * (50000 >> m_oss);
+ // printf("B7 = %d\n", B7);
+
+ int32_t p;
+ if (B7 < 0x80000000)
+ p = (B7 * 2) / B4;
+ else
+ p = (B7 / B4) * 2;
+
+ // printf("p = %d\n", p);
+ X1 = (p / 256) * (p / 256);
+ // printf("X1 = %d\n", X1);
+ X1 = (X1 * 3038) / 65536;
+ // printf("X1 = %d\n", X1);
+ X2 = (-7357 * p) / 65536;
+ // printf("X2 = %d\n", X2);
+ m_pressure = (RTFLOAT)(p + (X1 + X2 + 3791) / 16) / (RTFLOAT)100; // the extra 100 factor is to get 1hPa units
+
+ m_validReadings = true;
+
+ // printf("UP = %d, P = %f, UT = %d, T = %f\n", m_rawPressure, m_pressure, m_rawTemperature, m_temperature);
+ break;
+ }
+}
+
+void RTPressureBMP180::setTestData()
+{
+ m_AC1 = 408;
+ m_AC2 = -72;
+ m_AC3 = -14383;
+ m_AC4 = 32741;
+ m_AC5 = 32757;
+ m_AC6 = 23153;
+ m_B1 = 6190;
+ m_B2 = 4;
+ m_MB = -32767;
+ m_MC = -8711;
+ m_MD = 2868;
+
+ m_rawTemperature = 27898;
+ m_rawPressure = 23843;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
new file mode 100644
index 0000000..b1fb2e9
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
@@ -0,0 +1,88 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREBMP180_H_
+#define _RTPRESSUREBMP180_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define BMP180_STATE_IDLE 0
+#define BMP180_STATE_TEMPERATURE 1
+#define BMP180_STATE_PRESSURE 2
+
+// Conversion reg defs
+
+#define BMP180_SCO_TEMPCONV 0x2e // temperature conversion
+#define BMP180_SCO_PRESSURECONV_ULP 0 // ultra low power pressure conversion
+#define BMP180_SCO_PRESSURECONV_STD 1 // standard pressure conversion
+#define BMP180_SCO_PRESSURECONV_HR 2 // high res pressure conversion
+#define BMP180_SCO_PRESSURECONV_UHR 3 // ultra high res pressure conversion
+
+class RTIMUSettings;
+
+class RTPressureBMP180 : public RTPressure
+{
+public:
+ RTPressureBMP180(RTIMUSettings *settings);
+ ~RTPressureBMP180();
+
+ virtual const char *pressureName() { return "BMP180"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_BMP180; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ // This is the calibration data read from the sensor
+
+ int32_t m_AC1;
+ int32_t m_AC2;
+ int32_t m_AC3;
+ uint32_t m_AC4;
+ uint32_t m_AC5;
+ uint32_t m_AC6;
+ int32_t m_B1;
+ int32_t m_B2;
+ int32_t m_MB;
+ int32_t m_MC;
+ int32_t m_MD;
+
+ int m_state;
+ int m_oss;
+
+ uint16_t m_rawPressure;
+ uint16_t m_rawTemperature;
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREBMP180_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
new file mode 100644
index 0000000..6ece64f
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
@@ -0,0 +1,105 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREDEFS_H
+#define _RTPRESSUREDEFS_H
+
+// Pressure sensor type codes
+
+#define RTPRESSURE_TYPE_AUTODISCOVER 0 // audodiscover the pressure sensor
+#define RTPRESSURE_TYPE_NULL 1 // if no physical hardware
+#define RTPRESSURE_TYPE_BMP180 2 // BMP180
+#define RTPRESSURE_TYPE_LPS25H 3 // LPS25H
+#define RTPRESSURE_TYPE_MS5611 4 // MS5611
+#define RTPRESSURE_TYPE_MS5637 5 // MS5637
+
+//----------------------------------------------------------
+//
+// BMP180
+
+// BMP180 I2C Slave Addresses
+
+#define BMP180_ADDRESS 0x77
+#define BMP180_REG_ID 0xd0
+#define BMP180_ID 0x55
+
+// Register map
+
+#define BMP180_REG_AC1 0xaa
+#define BMP180_REG_SCO 0xf4
+#define BMP180_REG_RESULT 0xf6
+#define BMP180_REG_XLSB 0xf8
+
+//----------------------------------------------------------
+//
+// LPS25H
+
+// LPS25H I2C Slave Addresses
+
+#define LPS25H_ADDRESS0 0x5c
+#define LPS25H_ADDRESS1 0x5d
+#define LPS25H_REG_ID 0x0f
+#define LPS25H_ID 0xbd
+
+// Register map
+
+#define LPS25H_REF_P_XL 0x08
+#define LPS25H_REF_P_XH 0x09
+#define LPS25H_RES_CONF 0x10
+#define LPS25H_CTRL_REG_1 0x20
+#define LPS25H_CTRL_REG_2 0x21
+#define LPS25H_CTRL_REG_3 0x22
+#define LPS25H_CTRL_REG_4 0x23
+#define LPS25H_INT_CFG 0x24
+#define LPS25H_INT_SOURCE 0x25
+#define LPS25H_STATUS_REG 0x27
+#define LPS25H_PRESS_OUT_XL 0x28
+#define LPS25H_PRESS_OUT_L 0x29
+#define LPS25H_PRESS_OUT_H 0x2a
+#define LPS25H_TEMP_OUT_L 0x2b
+#define LPS25H_TEMP_OUT_H 0x2c
+#define LPS25H_FIFO_CTRL 0x2e
+#define LPS25H_FIFO_STATUS 0x2f
+#define LPS25H_THS_P_L 0x30
+#define LPS25H_THS_P_H 0x31
+#define LPS25H_RPDS_L 0x39
+#define LPS25H_RPDS_H 0x3a
+
+//----------------------------------------------------------
+//
+// MS5611 and MS5637
+
+// MS5611 I2C Slave Addresses
+
+#define MS5611_ADDRESS0 0x76
+#define MS5611_ADDRESS1 0x77
+
+// commands
+
+#define MS5611_CMD_RESET 0x1e
+#define MS5611_CMD_CONV_D1 0x48
+#define MS5611_CMD_CONV_D2 0x58
+#define MS5611_CMD_PROM 0xa0
+#define MS5611_CMD_ADC 0x00
+
+#endif // _RTPRESSUREDEFS_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
new file mode 100644
index 0000000..79cc813
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
@@ -0,0 +1,91 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureLPS25H.h"
+#include "RTPressureDefs.h"
+
+RTPressureLPS25H::RTPressureLPS25H(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_pressureValid = false;
+ m_temperatureValid = false;
+ }
+
+RTPressureLPS25H::~RTPressureLPS25H()
+{
+}
+
+bool RTPressureLPS25H::pressureInit()
+{
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_CTRL_REG_1, 0xc4, "Failed to set LPS25H CTRL_REG_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_RES_CONF, 0x05, "Failed to set LPS25H RES_CONF"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_FIFO_CTRL, 0xc0, "Failed to set LPS25H FIFO_CTRL"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_CTRL_REG_2, 0x40, "Failed to set LPS25H CTRL_REG_2"))
+ return false;
+
+ return true;
+}
+
+
+bool RTPressureLPS25H::pressureRead(RTIMU_DATA& data)
+{
+ unsigned char rawData[3];
+ unsigned char status;
+
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_STATUS_REG, 1, &status, "Failed to read LPS25H status"))
+ return false;
+
+ if (status & 2) {
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_PRESS_OUT_XL + 0x80, 3, rawData, "Failed to read LPS25H pressure"))
+ return false;
+
+ m_pressure = (RTFLOAT)((((unsigned int)rawData[2]) << 16) | (((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)4096;
+ m_pressureValid = true;
+ }
+ if (status & 1) {
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_TEMP_OUT_L + 0x80, 2, rawData, "Failed to read LPS25H temperature"))
+ return false;
+
+ m_temperature = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)480 + (RTFLOAT)42.5;
+ m_temperatureValid = true;
+ }
+
+ data.pressureValid = m_pressureValid;
+ data.pressure = m_pressure;
+ data.temperatureValid = m_temperatureValid;
+ data.temperature = m_temperature;
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
new file mode 100644
index 0000000..2b4a1a9
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
@@ -0,0 +1,53 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSURELPS25H_H_
+#define _RTPRESSURELPS25H_H_
+
+#include "RTPressure.h"
+
+class RTIMUSettings;
+
+class RTPressureLPS25H : public RTPressure
+{
+public:
+ RTPressureLPS25H(RTIMUSettings *settings);
+ ~RTPressureLPS25H();
+
+ virtual const char *pressureName() { return "LPS25H"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_LPS25H; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ unsigned char m_pressureAddr; // I2C address
+
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+ bool m_pressureValid;
+ bool m_temperatureValid;
+
+};
+
+#endif // _RTPRESSURELPS25H_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
new file mode 100644
index 0000000..3aa8824
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
@@ -0,0 +1,170 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureMS5611.h"
+
+RTPressureMS5611::RTPressureMS5611(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureMS5611::~RTPressureMS5611()
+{
+}
+
+bool RTPressureMS5611::pressureInit()
+{
+ unsigned char cmd = MS5611_CMD_PROM + 2;
+ unsigned char data[2];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // get calibration data
+
+ for (int i = 0; i < 6; i++) {
+ if (!m_settings->HALRead(m_pressureAddr, cmd, 2, data, "Failed to read MS5611 calibration data"))
+ return false;
+ m_calData[i] = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+ // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
+ cmd += 2;
+ }
+
+ m_state = MS5611_STATE_IDLE;
+ return true;
+}
+
+bool RTPressureMS5611::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == MS5611_STATE_IDLE) {
+ // start pressure conversion
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0, "Failed to start MS5611 pressure conversion")) {
+ return false;
+ } else {
+ m_state = MS5611_STATE_PRESSURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ }
+ return true;
+}
+
+
+void RTPressureMS5611::pressureBackground()
+{
+ uint8_t data[3];
+
+ switch (m_state) {
+ case MS5611_STATE_IDLE:
+ break;
+
+ case MS5611_STATE_PRESSURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 pressure")) {
+ break;
+ }
+ m_D1 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+ // start temperature conversion
+
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0, "Failed to start MS5611 temperature conversion")) {
+ break;
+ } else {
+ m_state = MS5611_STATE_TEMPERATURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ break;
+
+ case MS5611_STATE_TEMPERATURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 temperature")) {
+ break;
+ }
+ m_D2 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+ // call this function for testing only
+ // should give T = 2007 (20.07C) and pressure 100009 (1000.09hPa)
+
+ // setTestData();
+
+ // now calculate the real values
+
+ int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
+
+ int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this needs to be divided by 100
+
+ int64_t offset = ((int64_t)m_calData[1] << 16) + (((int64_t)m_calData[3] * deltaT) >> 7);
+ int64_t sens = ((int64_t)m_calData[0] << 15) + (((int64_t)m_calData[2] * deltaT) >> 8);
+
+ // do second order temperature compensation
+
+ if (temperature < 2000) {
+ int64_t T2 = (deltaT * deltaT) >> 31;
+ int64_t offset2 = 5 * ((temperature - 2000) * (temperature - 2000)) / 2;
+ int64_t sens2 = offset2 / 2;
+ if (temperature < -1500) {
+ offset2 += 7 * (temperature + 1500) * (temperature + 1500);
+ sens2 += 11 * ((temperature + 1500) * (temperature + 1500)) / 2;
+ }
+ temperature -= T2;
+ offset -= offset2;
+ sens -=sens2;
+ }
+
+ m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+ m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
+
+ // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
+
+ m_validReadings = true;
+ m_state = MS5611_STATE_IDLE;
+ break;
+ }
+}
+
+void RTPressureMS5611::setTestData()
+{
+ m_calData[0] = 40127;
+ m_calData[1] = 36924;
+ m_calData[2] = 23317;
+ m_calData[3] = 23282;
+ m_calData[4] = 33464;
+ m_calData[5] = 28312;
+
+ m_D1 = 9085466;
+ m_D2 = 8569150;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
new file mode 100644
index 0000000..ab83c14
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
@@ -0,0 +1,69 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREMS5611_H_
+#define _RTPRESSUREMS5611_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define MS5611_STATE_IDLE 0
+#define MS5611_STATE_TEMPERATURE 1
+#define MS5611_STATE_PRESSURE 2
+
+class RTIMUSettings;
+
+class RTPressureMS5611 : public RTPressure
+{
+public:
+ RTPressureMS5611(RTIMUSettings *settings);
+ ~RTPressureMS5611();
+
+ virtual const char *pressureName() { return "MS5611"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ int m_state;
+
+ uint16_t m_calData[6]; // calibration data
+
+ uint32_t m_D1;
+ uint32_t m_D2;
+
+ uint64_t m_timer; // used to time coversions
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREMS5611_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
new file mode 100644
index 0000000..018bbe0
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
@@ -0,0 +1,172 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureMS5637.h"
+
+RTPressureMS5637::RTPressureMS5637(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureMS5637::~RTPressureMS5637()
+{
+}
+
+bool RTPressureMS5637::pressureInit()
+{
+ unsigned char cmd = MS5611_CMD_PROM + 2;
+ unsigned char data[2];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // get calibration data
+
+ for (int i = 0; i < 6; i++) {
+ if (!m_settings->HALRead(m_pressureAddr, cmd, 2, data, "Failed to read MS5611 calibration data"))
+ return false;
+ m_calData[i] = (((uint16_t)data[0]) << 8) | ((uint16_t)data[1]);
+ // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
+ cmd += 2;
+ }
+
+ m_state = MS5637_STATE_IDLE;
+ return true;
+}
+
+bool RTPressureMS5637::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == MS5637_STATE_IDLE) {
+ // start pressure conversion
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0, "Failed to start MS5611 pressure conversion")) {
+ return false;
+ } else {
+ m_state = MS5637_STATE_PRESSURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ }
+ return true;
+}
+
+
+void RTPressureMS5637::pressureBackground()
+{
+ uint8_t data[3];
+
+ switch (m_state) {
+ case MS5637_STATE_IDLE:
+ break;
+
+ case MS5637_STATE_PRESSURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 pressure")) {
+ break;
+ }
+ m_D1 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
+
+ // start temperature conversion
+
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0, "Failed to start MS5611 temperature conversion")) {
+ break;
+ } else {
+ m_state = MS5637_STATE_TEMPERATURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ break;
+
+ case MS5637_STATE_TEMPERATURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 temperature")) {
+ break;
+ }
+ m_D2 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
+
+ // call this function for testing only
+ // should give T = 2000 (20.00C) and pressure 110002 (1100.02hPa)
+
+ // setTestData();
+
+ // now calculate the real values
+
+ int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
+
+ int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this still needs to be divided by 100
+
+ int64_t offset = (((int64_t)m_calData[1]) << 17) + ((m_calData[3] * deltaT) >> 6);
+ int64_t sens = (((int64_t)m_calData[0]) << 16) + ((m_calData[2] * deltaT) >> 7);
+
+ // do second order temperature compensation
+
+ if (temperature < 2000) {
+ int64_t T2 = (3 * (deltaT * deltaT)) >> 33;
+ int64_t offset2 = 61 * ((temperature - 2000) * (temperature - 2000)) / 16;
+ int64_t sens2 = 29 * ((temperature - 2000) * (temperature - 2000)) / 16;
+ if (temperature < -1500) {
+ offset2 += 17 * (temperature + 1500) * (temperature + 1500);
+ sens2 += 9 * ((temperature + 1500) * (temperature + 1500));
+ }
+ temperature -= T2;
+ offset -= offset2;
+ sens -=sens2;
+ } else {
+ temperature -= (5 * (deltaT * deltaT)) >> 38;
+ }
+
+ m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+ m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
+
+ // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
+
+ m_validReadings = true;
+ m_state = MS5637_STATE_IDLE;
+ break;
+ }
+}
+
+void RTPressureMS5637::setTestData()
+{
+ m_calData[0] = 46372;
+ m_calData[1] = 43981;
+ m_calData[2] = 29059;
+ m_calData[3] = 27842;
+ m_calData[4] = 31553;
+ m_calData[5] = 28165;
+
+ m_D1 = 6465444;
+ m_D2 = 8077636;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
new file mode 100644
index 0000000..3320d5d
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
@@ -0,0 +1,69 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREMS5637_H_
+#define _RTPRESSUREMS5637_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define MS5637_STATE_IDLE 0
+#define MS5637_STATE_TEMPERATURE 1
+#define MS5637_STATE_PRESSURE 2
+
+class RTIMUSettings;
+
+class RTPressureMS5637 : public RTPressure
+{
+public:
+ RTPressureMS5637(RTIMUSettings *settings);
+ ~RTPressureMS5637();
+
+ virtual const char *pressureName() { return "MS5637"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ int m_state;
+
+ uint16_t m_calData[6]; // calibration data
+
+ uint32_t m_D1;
+ uint32_t m_D2;
+
+ uint64_t m_timer; // used to time coversions
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREMS5637_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp b/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
new file mode 100644
index 0000000..362b38c
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
@@ -0,0 +1,137 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTFusion.h"
+#include "RTIMUHal.h"
+
+// The slerp power valule controls the influence of the measured state to correct the predicted state
+// 0 = measured state ignored (just gyros), 1 = measured state overrides predicted state.
+// In between 0 and 1 mixes the two conditions
+
+#define RTQF_SLERP_POWER (RTFLOAT)0.02;
+
+const char *RTFusion::m_fusionNameMap[] = {
+ "NULL",
+ "Kalman STATE4",
+ "RTQF"};
+
+RTFusion::RTFusion()
+{
+ m_debug = false;
+ m_firstTime = true;
+ m_enableGyro = true;
+ m_enableAccel = true;
+ m_enableCompass = true;
+
+ m_gravity.setScalar(0);
+ m_gravity.setX(0);
+ m_gravity.setY(0);
+ m_gravity.setZ(1);
+
+ m_slerpPower = RTQF_SLERP_POWER;
+}
+
+RTFusion::~RTFusion()
+{
+}
+
+void RTFusion::calculatePose(const RTVector3& accel, const RTVector3& mag, float magDeclination)
+{
+ RTQuaternion m;
+ RTQuaternion q;
+
+ if (m_enableAccel) {
+ accel.accelToEuler(m_measuredPose);
+ } else {
+ m_measuredPose = m_fusionPose;
+ m_measuredPose.setZ(0);
+ }
+
+ if (m_enableCompass && m_compassValid) {
+ q.fromEuler(m_measuredPose);
+ m.setScalar(0);
+ m.setX(mag.x());
+ m.setY(mag.y());
+ m.setZ(mag.z());
+
+ m = q * m * q.conjugate();
+ m_measuredPose.setZ(-atan2(m.y(), m.x()) - magDeclination);
+ } else {
+ m_measuredPose.setZ(m_fusionPose.z());
+ }
+
+ m_measuredQPose.fromEuler(m_measuredPose);
+
+ // check for quaternion aliasing. If the quaternion has the wrong sign
+ // the kalman filter will be very unhappy.
+
+ int maxIndex = -1;
+ RTFLOAT maxVal = -1000;
+
+ for (int i = 0; i < 4; i++) {
+ if (fabs(m_measuredQPose.data(i)) > maxVal) {
+ maxVal = fabs(m_measuredQPose.data(i));
+ maxIndex = i;
+ }
+ }
+
+ // if the biggest component has a different sign in the measured and kalman poses,
+ // change the sign of the measured pose to match.
+
+ if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) ||
+ ((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) {
+ m_measuredQPose.setScalar(-m_measuredQPose.scalar());
+ m_measuredQPose.setX(-m_measuredQPose.x());
+ m_measuredQPose.setY(-m_measuredQPose.y());
+ m_measuredQPose.setZ(-m_measuredQPose.z());
+ m_measuredQPose.toEuler(m_measuredPose);
+ }
+}
+
+
+RTVector3 RTFusion::getAccelResiduals()
+{
+ RTQuaternion rotatedGravity;
+ RTQuaternion fusedConjugate;
+ RTQuaternion qTemp;
+ RTVector3 residuals;
+
+ // do gravity rotation and subtraction
+
+ // create the conjugate of the pose
+
+ fusedConjugate = m_fusionQPose.conjugate();
+
+ // now do the rotation - takes two steps with qTemp as the intermediate variable
+
+ qTemp = m_gravity * m_fusionQPose;
+ rotatedGravity = fusedConjugate * qTemp;
+
+ // now adjust the measured accel and change the signs to make sense
+
+ residuals.setX(-(m_accel.x() - rotatedGravity.x()));
+ residuals.setY(-(m_accel.y() - rotatedGravity.y()));
+ residuals.setZ(-(m_accel.z() - rotatedGravity.z()));
+ return residuals;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusion.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusion.h b/thirdparty/RTIMULib/RTIMULib/RTFusion.h
new file mode 100644
index 0000000..8f0cbbc
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTFusion.h
@@ -0,0 +1,105 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTFUSION_H
+#define _RTFUSION_H
+
+#include "RTIMULibDefs.h"
+
+class RTIMUSettings;
+
+class RTFusion
+{
+public:
+
+ RTFusion();
+ virtual ~RTFusion();
+
+ // fusionType returns the type code of the fusion algorithm
+
+ virtual int fusionType() { return RTFUSION_TYPE_NULL; }
+
+ // the following function can be called to set the SLERP power
+
+ void setSlerpPower(RTFLOAT power) { m_slerpPower = power; }
+
+ // reset() resets the fusion state but keeps any setting changes (such as enables)
+
+ virtual void reset() {}
+
+ // newIMUData() should be called for subsequent updates
+ // the fusion fields are updated with the results
+
+ virtual void newIMUData(RTIMU_DATA& /* data */, const RTIMUSettings * /* settings */) {}
+
+ // This static function returns performs the type to name mapping
+
+ static const char *fusionName(int fusionType) { return m_fusionNameMap[fusionType]; }
+
+ // the following three functions control the influence of the gyro, accel and compass sensors
+
+ void setGyroEnable(bool enable) { m_enableGyro = enable;}
+ void setAccelEnable(bool enable) { m_enableAccel = enable; }
+ void setCompassEnable(bool enable) { m_enableCompass = enable;}
+
+ inline const RTVector3& getMeasuredPose() {return m_measuredPose;}
+ inline const RTQuaternion& getMeasuredQPose() {return m_measuredQPose;}
+
+ // getAccelResiduals() gets the residual after subtracting gravity
+
+ RTVector3 getAccelResiduals();
+
+ void setDebugEnable(bool enable) { m_debug = enable; }
+
+protected:
+ void calculatePose(const RTVector3& accel, const RTVector3& mag, float magDeclination); // generates pose from accels and mag
+
+ RTVector3 m_gyro; // current gyro sample
+ RTVector3 m_accel; // current accel sample
+ RTVector3 m_compass; // current compass sample
+
+ RTQuaternion m_measuredQPose; // quaternion form of pose from measurement
+ RTVector3 m_measuredPose; // vector form of pose from measurement
+ RTQuaternion m_fusionQPose; // quaternion form of pose from fusion
+ RTVector3 m_fusionPose; // vector form of pose from fusion
+
+ RTQuaternion m_gravity; // the gravity vector as a quaternion
+
+ RTFLOAT m_slerpPower; // a value 0 to 1 that controls measured state influence
+ RTQuaternion m_rotationDelta; // amount by which measured state differs from predicted
+ RTQuaternion m_rotationPower; // delta raised to the appopriate power
+ RTVector3 m_rotationUnitVector; // the vector part of the rotation delta
+
+ bool m_debug;
+ bool m_enableGyro; // enables gyro as input
+ bool m_enableAccel; // enables accel as input
+ bool m_enableCompass; // enables compass a input
+ bool m_compassValid; // true if compass data valid
+
+ bool m_firstTime; // if first time after reset
+ uint64_t m_lastFusionTime; // for delta time calculation
+
+ static const char *m_fusionNameMap[]; // the fusion name array
+};
+
+#endif // _RTFUSION_H
[7/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.cpp
new file mode 100644
index 0000000..792b6df
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.cpp
@@ -0,0 +1,142 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTHumidityHTS221.h"
+#include "RTHumidityDefs.h"
+
+RTHumidityHTS221::RTHumidityHTS221(RTIMUSettings *settings) : RTHumidity(settings)
+{
+ m_humidityValid = false;
+ m_temperatureValid = false;
+ }
+
+RTHumidityHTS221::~RTHumidityHTS221()
+{
+}
+
+bool RTHumidityHTS221::humidityInit()
+{
+ unsigned char rawData[2];
+ uint8_t H0_H_2 = 0;
+ uint8_t H1_H_2 = 0;
+ uint16_t T0_C_8 = 0;
+ uint16_t T1_C_8 = 0;
+ int16_t H0_T0_OUT = 0;
+ int16_t H1_T0_OUT = 0;
+ int16_t T0_OUT = 0;
+ int16_t T1_OUT = 0;
+ float H0, H1, T0, T1;
+
+ m_humidityAddr = m_settings->m_I2CHumidityAddress;
+
+ if (!m_settings->HALWrite(m_humidityAddr, HTS221_CTRL1, 0x87, "Failed to set HTS221 CTRL_REG_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_humidityAddr, HTS221_AV_CONF, 0x1b, "Failed to set HTS221 AV_CONF"))
+ return false;
+
+ // Get calibration data
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_T1_T0 + 0x80, 1, &rawData[1], "Failed to read HTS221 T1_T0"))
+ return false;
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_T0_C_8 + 0x80, 1, rawData, "Failed to read HTS221 T0_C_8"))
+ return false;
+ T0_C_8 = (((unsigned int)rawData[1] & 0x3 ) << 8) | (unsigned int)rawData[0];
+ T0 = (RTFLOAT)T0_C_8 / 8;
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_T1_C_8 + 0x80, 1, rawData, "Failed to read HTS221 T1_C_8"))
+ return false;
+ T1_C_8 = (unsigned int)(((uint16_t)(rawData[1] & 0xC) << 6) | (uint16_t)rawData[0]);
+ T1 = (RTFLOAT)T1_C_8 / 8;
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_T0_OUT + 0x80, 2, rawData, "Failed to read HTS221 T0_OUT"))
+ return false;
+ T0_OUT = (int16_t)(((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0];
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_T1_OUT + 0x80, 2, rawData, "Failed to read HTS221 T1_OUT"))
+ return false;
+ T1_OUT = (int16_t)(((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0];
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_H0_H_2 + 0x80, 1, &H0_H_2, "Failed to read HTS221 H0_H_2"))
+ return false;
+ H0 = (RTFLOAT)H0_H_2 / 2;
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_H1_H_2 + 0x80, 1, &H1_H_2, "Failed to read HTS221 H1_H_2"))
+ return false;
+ H1 = (RTFLOAT)H1_H_2 / 2;
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_H0_T0_OUT + 0x80, 2, rawData, "Failed to read HTS221 H0_T_OUT"))
+ return false;
+ H0_T0_OUT = (int16_t)(((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0];
+
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_H1_T0_OUT + 0x80, 2, rawData, "Failed to read HTS221 H1_T_OUT"))
+ return false;
+ H1_T0_OUT = (int16_t)(((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0];
+
+ m_temperature_m = (T1-T0)/(T1_OUT-T0_OUT);
+ m_temperature_c = T0-(m_temperature_m*T0_OUT);
+ m_humidity_m = (H1-H0)/(H1_T0_OUT-H0_T0_OUT);
+ m_humidity_c = (H0)-(m_humidity_m*H0_T0_OUT);
+
+ return true;
+}
+
+
+bool RTHumidityHTS221::humidityRead(RTIMU_DATA& data)
+{
+ unsigned char rawData[2];
+ unsigned char status;
+
+ data.humidityValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.humidity = 0;
+
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_STATUS, 1, &status, "Failed to read HTS221 status"))
+ return false;
+
+ if (status & 2) {
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_HUMIDITY_OUT_L + 0x80, 2, rawData, "Failed to read HTS221 humidity"))
+ return false;
+
+ m_humidity = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]);
+ m_humidity = m_humidity * m_humidity_m + m_humidity_c;
+ m_humidityValid = true;
+ }
+ if (status & 1) {
+ if (!m_settings->HALRead(m_humidityAddr, HTS221_TEMP_OUT_L + 0x80, 2, rawData, "Failed to read HTS221 temperature"))
+ return false;
+
+ m_temperature = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]);
+ m_temperature = m_temperature * m_temperature_m + m_temperature_c;
+ m_temperatureValid = true;
+ }
+
+ data.humidityValid = m_humidityValid;
+ data.humidity = m_humidity;
+ data.temperatureValid = m_temperatureValid;
+ data.temperature = m_temperature;
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.h
new file mode 100644
index 0000000..4737ad5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTS221.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 _RTHUMIDITYHTS221_H_
+#define _RTHUMIDITYHTS221_H_
+
+#include "RTHumidity.h"
+
+class RTIMUSettings;
+
+class RTHumidityHTS221 : public RTHumidity
+{
+public:
+ RTHumidityHTS221(RTIMUSettings *settings);
+ ~RTHumidityHTS221();
+
+ virtual const char *humidityName() { return "HTS221"; }
+ virtual int humidityType() { return RTHUMIDITY_TYPE_HTS221; }
+ virtual bool humidityInit();
+ virtual bool humidityRead(RTIMU_DATA& data);
+
+private:
+ unsigned char m_humidityAddr; // I2C address
+
+ RTFLOAT m_humidity; // the current humidity
+ RTFLOAT m_temperature; // the current temperature
+ RTFLOAT m_temperature_m; // temperature calibration slope
+ RTFLOAT m_temperature_c; // temperature calibration y intercept
+ RTFLOAT m_humidity_m; // humidity calibration slope
+ RTFLOAT m_humidity_c; // humidity calibration y intercept
+ bool m_humidityValid;
+ bool m_temperatureValid;
+
+};
+
+#endif // _RTHUMIDITYHTS221_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.cpp
new file mode 100644
index 0000000..d1380bb
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.cpp
@@ -0,0 +1,126 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTHumidityHTU21D.h"
+#include "RTHumidityDefs.h"
+
+#define HTU21D_STATE_IN_RESET 0 // reset in progress
+#define HTU21D_STATE_IDLE 1 // nothing happening
+#define HTU21D_STATE_TEMP_REQ 2 // requested temperature
+#define HTU21D_STATE_HUM_REQ 3 // requested humidity
+
+#define HTU21D_STATE_INTERVAL 100000 // the interval between state changes
+
+RTHumidityHTU21D::RTHumidityHTU21D(RTIMUSettings *settings) : RTHumidity(settings)
+{
+ m_humidityValid = false;
+ m_temperatureValid = false;
+ m_humidity = 0;
+ m_temperature = 0;
+ }
+
+RTHumidityHTU21D::~RTHumidityHTU21D()
+{
+}
+
+bool RTHumidityHTU21D::humidityInit()
+{
+ m_humidityAddr = m_settings->m_I2CHumidityAddress;
+
+ if (!m_settings->HALWrite(m_humidityAddr, HTU21D_CMD_SOFT_RESET, 0, NULL, "Failed to reset HTU21D"))
+ return false;
+
+ m_state = HTU21D_STATE_IN_RESET;
+ m_startTime = RTMath::currentUSecsSinceEpoch();
+ return true;
+}
+
+
+bool RTHumidityHTU21D::humidityRead(RTIMU_DATA& data)
+{
+ if (!processBackground())
+ return false;
+
+ data.humidityValid = m_humidityValid;
+ data.humidity = m_humidity;
+ data.temperatureValid = m_temperatureValid;
+ data.temperature = m_temperature;
+
+ return true;
+}
+
+bool RTHumidityHTU21D:: processBackground()
+{
+ unsigned char rawData[3];
+ uint64_t now = RTMath::currentUSecsSinceEpoch();
+ bool expired = (now - m_startTime) >= HTU21D_STATE_INTERVAL;
+
+ if (!expired)
+ return true;
+
+ switch (m_state) {
+ case HTU21D_STATE_IN_RESET:
+ m_state = HTU21D_STATE_IDLE;
+ m_startTime = now;
+ break;
+
+ case HTU21D_STATE_IDLE:
+ // start a temperature conversion
+ if (!m_settings->HALWrite(m_humidityAddr, HTU21D_CMD_TRIG_TEMP, 0, NULL, "Failed to start HTU21D temp conv"))
+ return false;
+ m_state = HTU21D_STATE_TEMP_REQ;
+ m_startTime = now;
+ break;
+
+ case HTU21D_STATE_TEMP_REQ:
+ // read temp data
+ if (!m_settings->HALRead(m_humidityAddr, 3, rawData, "Failed to read HTU21D temperature"))
+ return false;
+ // remove status bits
+ rawData[1] &= 0xfc;
+ m_temperature = -46.85 + 175.72 * (RTFLOAT)((((uint16_t)rawData[0]) << 8) | (uint16_t)rawData[1]) / 65536.0;
+ m_temperatureValid = true;
+
+ // start humidity conversion
+ if (!m_settings->HALWrite(m_humidityAddr, HTU21D_CMD_TRIG_HUM, 0, NULL, "Failed to start HTU21D humidity conv"))
+ return false;
+ m_state = HTU21D_STATE_HUM_REQ;
+ m_startTime = now;
+ break;
+
+ case HTU21D_STATE_HUM_REQ:
+ // read humidity data
+ if (!m_settings->HALRead(m_humidityAddr, 3, rawData, "Failed to read HTU21D humidity"))
+ return false;
+ // remove status bits
+ rawData[1] &= 0xfc;
+ m_humidity = -6.0 + 125.0 * (RTFLOAT)((((uint16_t)rawData[0]) << 8) | (uint16_t)rawData[1]) / 65536.0;
+ // do temp compensation
+ m_humidity += (25.0 - m_temperature) * -0.15;
+ m_humidityValid = true;
+ m_state = HTU21D_STATE_IDLE;
+ m_startTime = now;
+ break;
+ }
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.h
new file mode 100644
index 0000000..f632e39
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTHumidityHTU21D.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 _RTHUMIDITYHTU21D_H_
+#define _RTHUMIDITYHTU21D_H_
+
+#include "RTHumidity.h"
+
+class RTIMUSettings;
+
+class RTHumidityHTU21D : public RTHumidity
+{
+public:
+ RTHumidityHTU21D(RTIMUSettings *settings);
+ ~RTHumidityHTU21D();
+
+ virtual const char *humidityName() { return "HTU21D"; }
+ virtual int humidityType() { return RTHUMIDITY_TYPE_HTU21D; }
+ virtual bool humidityInit();
+ virtual bool humidityRead(RTIMU_DATA& data);
+
+private:
+ bool processBackground();
+
+ unsigned char m_humidityAddr; // I2C address
+
+ int m_state;
+ uint64_t m_startTime;
+ RTFLOAT m_humidity; // the current humidity
+ RTFLOAT m_temperature; // the current temperature
+ bool m_humidityValid;
+ bool m_temperatureValid;
+
+};
+
+#endif // _RTHUMIDITYHTU21D_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.cpp
new file mode 100644
index 0000000..3d36258
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.cpp
@@ -0,0 +1,478 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech
+//
+// 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 "RTIMU.h"
+#include "RTFusionKalman4.h"
+#include "RTFusionRTQF.h"
+
+#include "RTIMUNull.h"
+#include "RTIMUMPU9150.h"
+#include "RTIMUMPU9250.h"
+#include "RTIMUGD20HM303D.h"
+#include "RTIMUGD20M303DLHC.h"
+#include "RTIMUGD20HM303DLHC.h"
+#include "RTIMULSM9DS0.h"
+#include "RTIMULSM9DS1.h"
+#include "RTIMUBMX055.h"
+#include "RTIMUBNO055.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+// this sets the min range (max - min) of values to trigger runtime mag calibration
+
+#define RTIMU_RUNTIME_MAGCAL_RANGE 30
+
+// this defines the accelerometer noise level
+
+#define RTIMU_FUZZY_GYRO_ZERO 0.20
+
+// this defines the accelerometer noise level
+
+#define RTIMU_FUZZY_ACCEL_ZERO 0.05
+
+// Axis rotation arrays
+
+float RTIMU::m_axisRotation[RTIMU_AXIS_ROTATION_COUNT][9] = {
+ {1, 0, 0, 0, 1, 0, 0, 0, 1}, // RTIMU_XNORTH_YEAST
+ {0, -1, 0, 1, 0, 0, 0, 0, 1}, // RTIMU_XEAST_YSOUTH
+ {-1, 0, 0, 0, -1, 0, 0, 0, 1}, // RTIMU_XSOUTH_YWEST
+ {0, 1, 0, -1, 0, 0, 0, 0, 1}, // RTIMU_XWEST_YNORTH
+
+ {1, 0, 0, 0, -1, 0, 0, 0, -1}, // RTIMU_XNORTH_YWEST
+ {0, 1, 0, 1, 0, 0, 0, 0, -1}, // RTIMU_XEAST_YNORTH
+ {-1, 0, 0, 0, 1, 0, 0, 0, -1}, // RTIMU_XSOUTH_YEAST
+ {0, -1, 0, -1, 0, 0, 0, 0, -1}, // RTIMU_XWEST_YSOUTH
+
+ {0, 1, 0, 0, 0, -1, -1, 0, 0}, // RTIMU_XUP_YNORTH
+ {0, 0, 1, 0, 1, 0, -1, 0, 0}, // RTIMU_XUP_YEAST
+ {0, -1, 0, 0, 0, 1, -1, 0, 0}, // RTIMU_XUP_YSOUTH
+ {0, 0, -1, 0, -1, 0, -1, 0, 0}, // RTIMU_XUP_YWEST
+
+ {0, 1, 0, 0, 0, 1, 1, 0, 0}, // RTIMU_XDOWN_YNORTH
+ {0, 0, -1, 0, 1, 0, 1, 0, 0}, // RTIMU_XDOWN_YEAST
+ {0, -1, 0, 0, 0, -1, 1, 0, 0}, // RTIMU_XDOWN_YSOUTH
+ {0, 0, 1, 0, -1, 0, 1, 0, 0}, // RTIMU_XDOWN_YWEST
+
+ {1, 0, 0, 0, 0, 1, 0, -1, 0}, // RTIMU_XNORTH_YUP
+ {0, 0, -1, 1, 0, 0, 0, -1, 0}, // RTIMU_XEAST_YUP
+ {-1, 0, 0, 0, 0, -1, 0, -1, 0}, // RTIMU_XSOUTH_YUP
+ {0, 0, 1, -1, 0, 0, 0, -1, 0}, // RTIMU_XWEST_YUP
+
+ {1, 0, 0, 0, 0, -1, 0, 1, 0}, // RTIMU_XNORTH_YDOWN
+ {0, 0, 1, 1, 0, 0, 0, 1, 0}, // RTIMU_XEAST_YDOWN
+ {-1, 0, 0, 0, 0, 1, 0, 1, 0}, // RTIMU_XSOUTH_YDOWN
+ {0, 0, -1, -1, 0, 0, 0, 1, 0} // RTIMU_XWEST_YDOWN
+};
+
+RTIMU *RTIMU::createIMU(RTIMUSettings *settings)
+{
+ switch (settings->m_imuType) {
+ case RTIMU_TYPE_MPU9150:
+ return new RTIMUMPU9150(settings);
+
+ case RTIMU_TYPE_GD20HM303D:
+ return new RTIMUGD20HM303D(settings);
+
+ case RTIMU_TYPE_GD20M303DLHC:
+ return new RTIMUGD20M303DLHC(settings);
+
+ case RTIMU_TYPE_LSM9DS0:
+ return new RTIMULSM9DS0(settings);
+ case RTIMU_TYPE_LSM9DS1:
+ return new RTIMULSM9DS1(settings);
+
+ case RTIMU_TYPE_MPU9250:
+ return new RTIMUMPU9250(settings);
+
+ case RTIMU_TYPE_GD20HM303DLHC:
+ return new RTIMUGD20HM303DLHC(settings);
+
+ case RTIMU_TYPE_BMX055:
+ return new RTIMUBMX055(settings);
+
+ case RTIMU_TYPE_BNO055:
+ return new RTIMUBNO055(settings);
+
+ case RTIMU_TYPE_AUTODISCOVER:
+ if (settings->discoverIMU(settings->m_imuType, settings->m_busIsI2C, settings->m_I2CSlaveAddress)) {
+ settings->saveSettings();
+ return RTIMU::createIMU(settings);
+ }
+ return new RTIMUNull(settings);
+
+ case RTIMU_TYPE_NULL:
+ return new RTIMUNull(settings);
+
+ default:
+ return NULL;
+ }
+}
+
+
+RTIMU::RTIMU(RTIMUSettings *settings)
+{
+ m_settings = settings;
+
+ m_compassCalibrationMode = false;
+ m_accelCalibrationMode = false;
+
+ m_runtimeMagCalValid = false;
+
+ for (int i = 0; i < 3; i++) {
+ m_runtimeMagCalMax[i] = -1000;
+ m_runtimeMagCalMin[i] = 1000;
+ }
+
+ switch (m_settings->m_fusionType) {
+ case RTFUSION_TYPE_KALMANSTATE4:
+ m_fusion = new RTFusionKalman4();
+ break;
+
+ case RTFUSION_TYPE_RTQF:
+ m_fusion = new RTFusionRTQF();
+ break;
+
+ default:
+ m_fusion = new RTFusion();
+ break;
+ }
+ HAL_INFO1("Using fusion algorithm %s\n", RTFusion::fusionName(m_settings->m_fusionType));
+}
+
+RTIMU::~RTIMU()
+{
+ delete m_fusion;
+ m_fusion = NULL;
+}
+
+void RTIMU::setCalibrationData()
+{
+ float maxDelta = -1;
+ float delta;
+
+ if (m_settings->m_compassCalValid) {
+ // find biggest range
+
+ for (int i = 0; i < 3; i++) {
+ if ((m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i)) > maxDelta)
+ maxDelta = m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i);
+ }
+ if (maxDelta < 0) {
+ HAL_ERROR("Error in compass calibration data\n");
+ return;
+ }
+ maxDelta /= 2.0f; // this is the max +/- range
+
+ for (int i = 0; i < 3; i++) {
+ delta = (m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i)) / 2.0f;
+ m_compassCalScale[i] = maxDelta / delta; // makes everything the same range
+ m_compassCalOffset[i] = (m_settings->m_compassCalMax.data(i) + m_settings->m_compassCalMin.data(i)) / 2.0f;
+ }
+ }
+
+ if (m_settings->m_compassCalValid) {
+ HAL_INFO("Using min/max compass calibration\n");
+ } else {
+ HAL_INFO("min/max compass calibration not in use\n");
+ }
+
+ if (m_settings->m_compassCalEllipsoidValid) {
+ HAL_INFO("Using ellipsoid compass calibration\n");
+ } else {
+ HAL_INFO("Ellipsoid compass calibration not in use\n");
+ }
+
+ if (m_settings->m_accelCalValid) {
+ HAL_INFO("Using accel calibration\n");
+ } else {
+ HAL_INFO("Accel calibration not in use\n");
+ }
+}
+
+bool RTIMU::setGyroContinuousLearningAlpha(RTFLOAT alpha)
+{
+ if ((alpha < 0.0) || (alpha >= 1.0))
+ return false;
+
+ m_gyroContinuousAlpha = alpha;
+ return true;
+}
+
+
+void RTIMU::gyroBiasInit()
+{
+ m_gyroLearningAlpha = 2.0f / m_sampleRate;
+ m_gyroContinuousAlpha = 0.01f / m_sampleRate;
+ m_gyroSampleCount = 0;
+}
+
+// Note - code assumes that this is the first thing called after axis swapping
+// for each specific IMU chip has occurred.
+
+void RTIMU::handleGyroBias()
+{
+ // do axis rotation
+
+ if ((m_settings->m_axisRotation > 0) && (m_settings->m_axisRotation < RTIMU_AXIS_ROTATION_COUNT)) {
+ // need to do an axis rotation
+ float *matrix = m_axisRotation[m_settings->m_axisRotation];
+ RTIMU_DATA tempIMU = m_imuData;
+
+ // do new x value
+ if (matrix[0] != 0) {
+ m_imuData.gyro.setX(tempIMU.gyro.x() * matrix[0]);
+ m_imuData.accel.setX(tempIMU.accel.x() * matrix[0]);
+ m_imuData.compass.setX(tempIMU.compass.x() * matrix[0]);
+ } else if (matrix[1] != 0) {
+ m_imuData.gyro.setX(tempIMU.gyro.y() * matrix[1]);
+ m_imuData.accel.setX(tempIMU.accel.y() * matrix[1]);
+ m_imuData.compass.setX(tempIMU.compass.y() * matrix[1]);
+ } else if (matrix[2] != 0) {
+ m_imuData.gyro.setX(tempIMU.gyro.z() * matrix[2]);
+ m_imuData.accel.setX(tempIMU.accel.z() * matrix[2]);
+ m_imuData.compass.setX(tempIMU.compass.z() * matrix[2]);
+ }
+
+ // do new y value
+ if (matrix[3] != 0) {
+ m_imuData.gyro.setY(tempIMU.gyro.x() * matrix[3]);
+ m_imuData.accel.setY(tempIMU.accel.x() * matrix[3]);
+ m_imuData.compass.setY(tempIMU.compass.x() * matrix[3]);
+ } else if (matrix[4] != 0) {
+ m_imuData.gyro.setY(tempIMU.gyro.y() * matrix[4]);
+ m_imuData.accel.setY(tempIMU.accel.y() * matrix[4]);
+ m_imuData.compass.setY(tempIMU.compass.y() * matrix[4]);
+ } else if (matrix[5] != 0) {
+ m_imuData.gyro.setY(tempIMU.gyro.z() * matrix[5]);
+ m_imuData.accel.setY(tempIMU.accel.z() * matrix[5]);
+ m_imuData.compass.setY(tempIMU.compass.z() * matrix[5]);
+ }
+
+ // do new z value
+ if (matrix[6] != 0) {
+ m_imuData.gyro.setZ(tempIMU.gyro.x() * matrix[6]);
+ m_imuData.accel.setZ(tempIMU.accel.x() * matrix[6]);
+ m_imuData.compass.setZ(tempIMU.compass.x() * matrix[6]);
+ } else if (matrix[7] != 0) {
+ m_imuData.gyro.setZ(tempIMU.gyro.y() * matrix[7]);
+ m_imuData.accel.setZ(tempIMU.accel.y() * matrix[7]);
+ m_imuData.compass.setZ(tempIMU.compass.y() * matrix[7]);
+ } else if (matrix[8] != 0) {
+ m_imuData.gyro.setZ(tempIMU.gyro.z() * matrix[8]);
+ m_imuData.accel.setZ(tempIMU.accel.z() * matrix[8]);
+ m_imuData.compass.setZ(tempIMU.compass.z() * matrix[8]);
+ }
+ }
+
+ RTVector3 deltaAccel = m_previousAccel;
+ deltaAccel -= m_imuData.accel; // compute difference
+ m_previousAccel = m_imuData.accel;
+
+ if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (m_imuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO)) {
+ // what we are seeing on the gyros should be bias only so learn from this
+
+ if (m_gyroSampleCount < (5 * m_sampleRate)) {
+ m_settings->m_gyroBias.setX((1.0 - m_gyroLearningAlpha) * m_settings->m_gyroBias.x() + m_gyroLearningAlpha * m_imuData.gyro.x());
+ m_settings->m_gyroBias.setY((1.0 - m_gyroLearningAlpha) * m_settings->m_gyroBias.y() + m_gyroLearningAlpha * m_imuData.gyro.y());
+ m_settings->m_gyroBias.setZ((1.0 - m_gyroLearningAlpha) * m_settings->m_gyroBias.z() + m_gyroLearningAlpha * m_imuData.gyro.z());
+
+ m_gyroSampleCount++;
+
+ if (m_gyroSampleCount == (5 * m_sampleRate)) {
+ // this could have been true already of course
+ m_settings->m_gyroBiasValid = true;
+ m_settings->saveSettings();
+ }
+ } else {
+ m_settings->m_gyroBias.setX((1.0 - m_gyroContinuousAlpha) * m_settings->m_gyroBias.x() + m_gyroContinuousAlpha * m_imuData.gyro.x());
+ m_settings->m_gyroBias.setY((1.0 - m_gyroContinuousAlpha) * m_settings->m_gyroBias.y() + m_gyroContinuousAlpha * m_imuData.gyro.y());
+ m_settings->m_gyroBias.setZ((1.0 - m_gyroContinuousAlpha) * m_settings->m_gyroBias.z() + m_gyroContinuousAlpha * m_imuData.gyro.z());
+ }
+ }
+
+ m_imuData.gyro -= m_settings->m_gyroBias;
+}
+
+void RTIMU::calibrateAverageCompass()
+{
+ // see if need to do runtime mag calibration (i.e. no stored calibration data)
+
+ if (!m_compassCalibrationMode && !m_settings->m_compassCalValid) {
+ // try runtime calibration
+ bool changed = false;
+
+ // see if there is a new max or min
+
+ if (m_runtimeMagCalMax[0] < m_imuData.compass.x()) {
+ m_runtimeMagCalMax[0] = m_imuData.compass.x();
+ changed = true;
+ }
+ if (m_runtimeMagCalMax[1] < m_imuData.compass.y()) {
+ m_runtimeMagCalMax[1] = m_imuData.compass.y();
+ changed = true;
+ }
+ if (m_runtimeMagCalMax[2] < m_imuData.compass.z()) {
+ m_runtimeMagCalMax[2] = m_imuData.compass.z();
+ changed = true;
+ }
+
+ if (m_runtimeMagCalMin[0] > m_imuData.compass.x()) {
+ m_runtimeMagCalMin[0] = m_imuData.compass.x();
+ changed = true;
+ }
+ if (m_runtimeMagCalMin[1] > m_imuData.compass.y()) {
+ m_runtimeMagCalMin[1] = m_imuData.compass.y();
+ changed = true;
+ }
+ if (m_runtimeMagCalMin[2] > m_imuData.compass.z()) {
+ m_runtimeMagCalMin[2] = m_imuData.compass.z();
+ changed = true;
+ }
+
+ // now see if ranges are sufficient
+
+ if (changed) {
+
+ float delta;
+
+ if (!m_runtimeMagCalValid) {
+ m_runtimeMagCalValid = true;
+
+ for (int i = 0; i < 3; i++)
+ {
+ delta = m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i];
+ if ((delta < RTIMU_RUNTIME_MAGCAL_RANGE) || (m_runtimeMagCalMin[i] > 0) || (m_runtimeMagCalMax[i] < 0))
+ {
+ m_runtimeMagCalValid = false;
+ break;
+ }
+ }
+ }
+
+ // find biggest range and scale to that
+
+ if (m_runtimeMagCalValid) {
+ float magMaxDelta = -1;
+
+ for (int i = 0; i < 3; i++) {
+ if ((m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i]) > magMaxDelta)
+ {
+ magMaxDelta = m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i];
+ }
+ }
+
+ // adjust for + and - range
+
+ magMaxDelta /= 2.0;
+
+ for (int i = 0; i < 3; i++)
+ {
+ delta = (m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i]) / 2.0;
+ m_compassCalScale[i] = magMaxDelta / delta;
+ m_compassCalOffset[i] = (m_runtimeMagCalMax[i] + m_runtimeMagCalMin[i]) / 2.0;
+ }
+ }
+ }
+ }
+
+ if (getCompassCalibrationValid() || getRuntimeCompassCalibrationValid()) {
+ m_imuData.compass.setX((m_imuData.compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
+ m_imuData.compass.setY((m_imuData.compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
+ m_imuData.compass.setZ((m_imuData.compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
+
+ if (m_settings->m_compassCalEllipsoidValid) {
+ RTVector3 ev = m_imuData.compass;
+ ev -= m_settings->m_compassCalEllipsoidOffset;
+
+ m_imuData.compass.setX(ev.x() * m_settings->m_compassCalEllipsoidCorr[0][0] +
+ ev.y() * m_settings->m_compassCalEllipsoidCorr[0][1] +
+ ev.z() * m_settings->m_compassCalEllipsoidCorr[0][2]);
+
+ m_imuData.compass.setY(ev.x() * m_settings->m_compassCalEllipsoidCorr[1][0] +
+ ev.y() * m_settings->m_compassCalEllipsoidCorr[1][1] +
+ ev.z() * m_settings->m_compassCalEllipsoidCorr[1][2]);
+
+ m_imuData.compass.setZ(ev.x() * m_settings->m_compassCalEllipsoidCorr[2][0] +
+ ev.y() * m_settings->m_compassCalEllipsoidCorr[2][1] +
+ ev.z() * m_settings->m_compassCalEllipsoidCorr[2][2]);
+ }
+ }
+
+ // update running average
+
+ m_compassAverage.setX(m_imuData.compass.x() * COMPASS_ALPHA + m_compassAverage.x() * (1.0 - COMPASS_ALPHA));
+ m_compassAverage.setY(m_imuData.compass.y() * COMPASS_ALPHA + m_compassAverage.y() * (1.0 - COMPASS_ALPHA));
+ m_compassAverage.setZ(m_imuData.compass.z() * COMPASS_ALPHA + m_compassAverage.z() * (1.0 - COMPASS_ALPHA));
+
+ m_imuData.compass = m_compassAverage;
+}
+
+void RTIMU::calibrateAccel()
+{
+ if (!getAccelCalibrationValid())
+ return;
+
+ if (m_imuData.accel.x() >= 0)
+ m_imuData.accel.setX(m_imuData.accel.x() / m_settings->m_accelCalMax.x());
+ else
+ m_imuData.accel.setX(m_imuData.accel.x() / -m_settings->m_accelCalMin.x());
+
+ if (m_imuData.accel.y() >= 0)
+ m_imuData.accel.setY(m_imuData.accel.y() / m_settings->m_accelCalMax.y());
+ else
+ m_imuData.accel.setY(m_imuData.accel.y() / -m_settings->m_accelCalMin.y());
+
+ if (m_imuData.accel.z() >= 0)
+ m_imuData.accel.setZ(m_imuData.accel.z() / m_settings->m_accelCalMax.z());
+ else
+ m_imuData.accel.setZ(m_imuData.accel.z() / -m_settings->m_accelCalMin.z());
+}
+
+void RTIMU::updateFusion()
+{
+ m_fusion->newIMUData(m_imuData, m_settings);
+}
+
+bool RTIMU::IMUGyroBiasValid()
+{
+ return m_settings->m_gyroBiasValid;
+}
+
+ void RTIMU::setExtIMUData(RTFLOAT gx, RTFLOAT gy, RTFLOAT gz, RTFLOAT ax, RTFLOAT ay, RTFLOAT az,
+ RTFLOAT mx, RTFLOAT my, RTFLOAT mz, uint64_t timestamp)
+ {
+ m_imuData.gyro.setX(gx);
+ m_imuData.gyro.setY(gy);
+ m_imuData.gyro.setZ(gz);
+ m_imuData.accel.setX(ax);
+ m_imuData.accel.setY(ay);
+ m_imuData.accel.setZ(az);
+ m_imuData.compass.setX(mx);
+ m_imuData.compass.setY(my);
+ m_imuData.compass.setZ(mz);
+ m_imuData.timestamp = timestamp;
+ updateFusion();
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.h
new file mode 100644
index 0000000..c950d27
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMU.h
@@ -0,0 +1,200 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMU_H
+#define _RTIMU_H
+
+#include "RTMath.h"
+#include "RTFusion.h"
+#include "RTIMULibDefs.h"
+#include "RTIMUSettings.h"
+
+// Axis rotation defs
+//
+// These allow the IMU to be virtually repositioned if it is in a non-standard configuration
+// Standard configuration is X pointing at north, Y pointing east and Z pointing down
+// with the IMU horizontal. There are 24 different possible orientations as defined
+// below. Setting the axis rotation code to non-zero values performs the repositioning.
+
+#define RTIMU_XNORTH_YEAST 0 // this is the default identity matrix
+#define RTIMU_XEAST_YSOUTH 1
+#define RTIMU_XSOUTH_YWEST 2
+#define RTIMU_XWEST_YNORTH 3
+#define RTIMU_XNORTH_YWEST 4
+#define RTIMU_XEAST_YNORTH 5
+#define RTIMU_XSOUTH_YEAST 6
+#define RTIMU_XWEST_YSOUTH 7
+#define RTIMU_XUP_YNORTH 8
+#define RTIMU_XUP_YEAST 9
+#define RTIMU_XUP_YSOUTH 10
+#define RTIMU_XUP_YWEST 11
+#define RTIMU_XDOWN_YNORTH 12
+#define RTIMU_XDOWN_YEAST 13
+#define RTIMU_XDOWN_YSOUTH 14
+#define RTIMU_XDOWN_YWEST 15
+#define RTIMU_XNORTH_YUP 16
+#define RTIMU_XEAST_YUP 17
+#define RTIMU_XSOUTH_YUP 18
+#define RTIMU_XWEST_YUP 19
+#define RTIMU_XNORTH_YDOWN 20
+#define RTIMU_XEAST_YDOWN 21
+#define RTIMU_XSOUTH_YDOWN 22
+#define RTIMU_XWEST_YDOWN 23
+
+#define RTIMU_AXIS_ROTATION_COUNT 24
+
+class RTIMU
+{
+public:
+ // IMUs should always be created with the following call
+
+ static RTIMU *createIMU(RTIMUSettings *settings);
+
+ // Constructor/destructor
+
+ RTIMU(RTIMUSettings *settings);
+ virtual ~RTIMU();
+
+ // These functions must be provided by sub classes
+
+ virtual const char *IMUName() = 0; // the name of the IMU
+ virtual int IMUType() = 0; // the type code of the IMU
+ virtual bool IMUInit() = 0; // set up the IMU
+ virtual int IMUGetPollInterval() = 0; // returns the recommended poll interval in mS
+ virtual bool IMURead() = 0; // get a sample
+
+ // setGyroContinuousALearninglpha allows the continuous learning rate to be over-ridden
+ // The value must be between 0.0 and 1.0 and will generally be close to 0
+
+ bool setGyroContinuousLearningAlpha(RTFLOAT alpha);
+
+ // returns true if enough samples for valid data
+
+ virtual bool IMUGyroBiasValid();
+
+ // the following function can be called to set the SLERP power
+
+ void setSlerpPower(RTFLOAT power) { m_fusion->setSlerpPower(power); }
+
+ // call the following to reset the fusion algorithm
+
+ void resetFusion() { m_fusion->reset(); }
+
+ // the following three functions control the influence of the gyro, accel and compass sensors
+
+ void setGyroEnable(bool enable) { m_fusion->setGyroEnable(enable);}
+ void setAccelEnable(bool enable) { m_fusion->setAccelEnable(enable);}
+ void setCompassEnable(bool enable) { m_fusion->setCompassEnable(enable);}
+
+ // call the following to enable debug messages
+
+ void setDebugEnable(bool enable) { m_fusion->setDebugEnable(enable); }
+
+ // getIMUData returns the standard outputs of the IMU and fusion filter
+
+ const RTIMU_DATA& getIMUData() { return m_imuData; }
+
+ // setExtIMUData allows data from some external IMU to be injected to the fusion algorithm
+
+ void setExtIMUData(RTFLOAT gx, RTFLOAT gy, RTFLOAT gz, RTFLOAT ax, RTFLOAT ay, RTFLOAT az,
+ RTFLOAT mx, RTFLOAT my, RTFLOAT mz, uint64_t timestamp);
+
+ // the following two functions get access to the measured pose (accel and compass)
+
+ const RTVector3& getMeasuredPose() { return m_fusion->getMeasuredPose(); }
+ const RTQuaternion& getMeasuredQPose() { return m_fusion->getMeasuredQPose(); }
+
+ // setCompassCalibrationMode() turns off use of cal data so that raw data can be accumulated
+ // to derive calibration data
+
+ void setCompassCalibrationMode(bool enable) { m_compassCalibrationMode = enable; }
+
+ // setAccelCalibrationMode() turns off use of cal data so that raw data can be accumulated
+ // to derive calibration data
+
+ void setAccelCalibrationMode(bool enable) { m_accelCalibrationMode = enable; }
+
+ // setCalibrationData configures the cal data from settings and also enables use if valid
+
+ void setCalibrationData();
+
+ // getCompassCalibrationValid() returns true if the compass min/max calibration data is being used
+
+ bool getCompassCalibrationValid() { return !m_compassCalibrationMode && m_settings->m_compassCalValid; }
+
+ // getRuntimeCompassCalibrationValid() returns true if the runtime compass min/max calibration data is being used
+
+ bool getRuntimeCompassCalibrationValid() { return !m_compassCalibrationMode && m_runtimeMagCalValid; }
+
+ // getCompassCalibrationEllipsoidValid() returns true if the compass ellipsoid calibration data is being used
+
+ bool getCompassCalibrationEllipsoidValid() { return !m_compassCalibrationMode && m_settings->m_compassCalEllipsoidValid; }
+
+ // getAccelCalibrationValid() returns true if the accel calibration data is being used
+
+ bool getAccelCalibrationValid() { return !m_accelCalibrationMode && m_settings->m_accelCalValid; }
+
+ const RTVector3& getGyro() { return m_imuData.gyro; } // gets gyro rates in radians/sec
+ const RTVector3& getAccel() { return m_imuData.accel; } // get accel data in gs
+ const RTVector3& getCompass() { return m_imuData.compass; } // gets compass data in uT
+
+ RTVector3 getAccelResiduals() { return m_fusion->getAccelResiduals(); }
+
+protected:
+ void gyroBiasInit(); // sets up gyro bias calculation
+ void handleGyroBias(); // adjust gyro for bias
+ void calibrateAverageCompass(); // calibrate and smooth compass
+ void calibrateAccel(); // calibrate the accelerometers
+ void updateFusion(); // call when new data to update fusion state
+
+ bool m_compassCalibrationMode; // true if cal mode so don't use cal data!
+ bool m_accelCalibrationMode; // true if cal mode so don't use cal data!
+
+ RTIMU_DATA m_imuData; // the data from the IMU
+
+ RTIMUSettings *m_settings; // the settings object pointer
+
+ RTFusion *m_fusion; // the fusion algorithm
+
+ int m_sampleRate; // samples per second
+ uint64_t m_sampleInterval; // interval between samples in microseonds
+
+ RTFLOAT m_gyroLearningAlpha; // gyro bias rapid learning rate
+ RTFLOAT m_gyroContinuousAlpha; // gyro bias continuous (slow) learning rate
+ int m_gyroSampleCount; // number of gyro samples used
+
+ RTVector3 m_previousAccel; // previous step accel for gyro learning
+
+ float m_compassCalOffset[3];
+ float m_compassCalScale[3];
+ RTVector3 m_compassAverage; // a running average to smooth the mag outputs
+
+ bool m_runtimeMagCalValid; // true if the runtime mag calibration has valid data
+ float m_runtimeMagCalMax[3]; // runtime max mag values seen
+ float m_runtimeMagCalMin[3]; // runtime min mag values seen
+
+ static float m_axisRotation[RTIMU_AXIS_ROTATION_COUNT][9]; // array of rotation matrices
+
+ };
+
+#endif // _RTIMU_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.cpp
new file mode 100644
index 0000000..6d06688
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.cpp
@@ -0,0 +1,971 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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.
+
+// Some code from Bosch BMM150 API driver:
+
+/*
+****************************************************************************
+* Copyright (C) 2011 - 2014 Bosch Sensortec GmbH
+*
+* bmm050.c
+* Date: 2014/12/12
+* Revision: 2.0.3 $
+*
+* Usage: Sensor Driver for BMM050 and BMM150 sensor
+*
+****************************************************************************
+* License:
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+*
+* 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.
+*
+* Neither the name of the copyright holder nor the names of the
+* contributors may be used to endorse or promote products derived from
+* this software without specific prior written permission.
+*
+* 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 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
+*
+* The information provided is believed to be accurate and reliable.
+* The copyright holder assumes no responsibility
+* for the consequences of use
+* of such information nor for any infringement of patents or
+* other rights of third parties which may result from its use.
+* No license is granted by implication or otherwise under any patent or
+* patent rights of the copyright holder.
+**************************************************************************/
+/****************************************************************************/
+
+#include "RTIMUBMX055.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUBMX055::RTIMUBMX055(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+}
+
+RTIMUBMX055::~RTIMUBMX055()
+{
+}
+
+bool RTIMUBMX055::IMUInit()
+{
+ unsigned char result;
+
+ m_firstTime = true;
+
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_WHO_AM_I, 1, &result, "Failed to read BMX055 gyro id"))
+ return false;
+
+ if (result != BMX055_GYRO_ID) {
+ HAL_ERROR1("Incorrect BMX055 id %d\n", result);
+ return false;
+ }
+
+ // work out accel address
+
+ if (m_settings->HALRead(BMX055_ACCEL_ADDRESS0, BMX055_ACCEL_WHO_AM_I, 1, &result, "")) {
+ if (result == BMX055_ACCEL_ID) {
+ m_accelSlaveAddr = BMX055_ACCEL_ADDRESS0;
+ } else {
+ m_accelSlaveAddr = BMX055_ACCEL_ADDRESS1;
+ }
+ }
+
+ // work out mag address
+
+ int magAddr;
+
+ for (magAddr = BMX055_MAG_ADDRESS0; magAddr <= BMX055_MAG_ADDRESS3; magAddr++) {
+
+ // have to enable chip to get id...
+
+ m_settings->HALWrite(magAddr, BMX055_MAG_POWER, 1, "");
+
+ m_settings->delayMs(50);
+
+ if (m_settings->HALRead(magAddr, BMX055_MAG_WHO_AM_I, 1, &result, "")) {
+ if (result == BMX055_MAG_ID) {
+ m_magSlaveAddr = magAddr;
+ break;
+ }
+ }
+ }
+
+ if (magAddr > BMX055_MAG_ADDRESS3) {
+ HAL_ERROR("Failed to find BMX055 mag\n");
+ return false;
+ }
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
+ return false;
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroFSR())
+ return false;
+
+ gyroBiasInit();
+
+ // set up the accel
+
+ if (!setAccelSampleRate())
+ return false;
+
+ if (!setAccelFSR())
+ return false;
+
+
+ // set up the mag
+
+ magInitTrimRegisters();
+ setMagPreset();
+
+ HAL_INFO("BMX055 init complete\n");
+ return true;
+}
+
+bool RTIMUBMX055::setGyroSampleRate()
+{
+ unsigned char bw;
+
+ switch (m_settings->m_BMX055GyroSampleRate) {
+ case BMX055_GYRO_SAMPLERATE_2000_523:
+ bw = 0;
+ m_sampleRate = 2000;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_2000_230:
+ bw = 1;
+ m_sampleRate = 2000;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_1000_116:
+ bw = 2;
+ m_sampleRate = 1000;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_400_47:
+ bw = 3;
+ m_sampleRate = 400;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_200_23:
+ bw = 4;
+ m_sampleRate = 200;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_100_12:
+ bw = 5;
+ m_sampleRate = 100;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_200_64:
+ bw = 6;
+ m_sampleRate = 200;
+ break;
+
+ case BMX055_GYRO_SAMPLERATE_100_32:
+ bw = 7;
+ m_sampleRate = 100;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal BMX055 gyro sample rate code %d\n", m_settings->m_BMX055GyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+ return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_BW, bw, "Failed to set BMX055 gyro rate"));
+}
+
+bool RTIMUBMX055::setGyroFSR()
+{
+ switch(m_settings->m_BMX055GyroFsr) {
+ case BMX055_GYRO_FSR_2000:
+ m_gyroScale = 0.061 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case BMX055_GYRO_FSR_1000:
+ m_gyroScale = 0.0305 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case BMX055_GYRO_FSR_500:
+ m_gyroScale = 0.0153 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case BMX055_GYRO_FSR_250:
+ m_gyroScale = 0.0076 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case BMX055_GYRO_FSR_125:
+ m_gyroScale = 0.0038 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal BMX055 gyro FSR code %d\n", m_settings->m_BMX055GyroFsr);
+ return false;
+
+ }
+ return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_RANGE, m_settings->m_BMX055GyroFsr, "Failed to set BMX055 gyro rate"));
+}
+
+
+bool RTIMUBMX055::setAccelSampleRate()
+{
+ unsigned char reg;
+
+ switch(m_settings->m_BMX055AccelSampleRate) {
+ case BMX055_ACCEL_SAMPLERATE_15:
+ reg = 0x08;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_31:
+ reg = 0x09;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_62:
+ reg = 0x0a;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_125:
+ reg = 0x0b;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_250:
+ reg = 0x0c;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_500:
+ reg = 0x0d;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_1000:
+ reg = 0x0e;
+ break;
+
+ case BMX055_ACCEL_SAMPLERATE_2000:
+ reg = 0x0f;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelSampleRate);
+ return false;
+ }
+ return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_BW, reg, "Failed to set BMX055 accel rate"));
+}
+
+bool RTIMUBMX055::setAccelFSR()
+{
+ unsigned char reg;
+
+ switch(m_settings->m_BMX055AccelFsr) {
+ case BMX055_ACCEL_FSR_2:
+ reg = 0x03;
+ m_accelScale = 0.00098 / 16.0;
+ break;
+
+ case BMX055_ACCEL_FSR_4:
+ reg = 0x05;
+ m_accelScale = 0.00195 / 16.0;
+ break;
+
+ case BMX055_ACCEL_FSR_8:
+ reg = 0x08;
+ m_accelScale = 0.00391 / 16.0;
+ break;
+
+ case BMX055_ACCEL_FSR_16:
+ reg = 0x0c;
+ m_accelScale = 0.00781 / 16.0;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelFsr);
+ return false;
+ }
+ return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_RANGE, reg, "Failed to set BMX055 accel rate"));
+}
+
+bool RTIMUBMX055::setMagPreset()
+{
+ unsigned char mode, repXY, repZ;
+
+ switch (m_settings->m_BMX055MagPreset) {
+ case BMX055_MAG_LOW_POWER: // ODR=10, RepXY=3, RepZ=3
+ mode = 0;
+ repXY = 1;
+ repZ = 2;
+ break;
+
+ case BMX055_MAG_REGULAR: // ODR=10, RepXY=9, RepZ=15
+ mode = 0;
+ repXY = 4;
+ repZ = 14;
+ break;
+
+ case BMX055_MAG_ENHANCED: // ODR=10, RepXY=15, RepZ=27
+ mode = 0;
+ repXY = 7;
+ repZ = 26;
+ break;
+
+ case BMX055_MAG_HIGH_ACCURACY: // ODR=10, RepXY=47, RepZ=83
+ mode = 0;
+ repXY = 23;
+ repZ = 82;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal BMX055 mag preset code %d\n", m_settings->m_BMX055MagPreset);
+ return false;
+ }
+
+ if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_MODE, mode, "Failed to set BMX055 mag mode"))
+ return false;
+ if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPXY, repXY, "Failed to set BMX055 mag repXY"))
+ return false;
+ if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPZ, repZ, "Failed to set BMX055 mag repZ"))
+ return false;
+ return true;
+}
+
+int RTIMUBMX055::IMUGetPollInterval()
+{
+ if (m_sampleRate > 400)
+ return 1;
+ else
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUBMX055::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char magData[8];
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_STATUS, 1, &status, "Failed to read BMX055 gyro fifo status"))
+ return false;
+
+ if (status & 0x80) {
+ // fifo overflowed
+ HAL_ERROR("BMX055 fifo overflow\n");
+
+ // this should clear it
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); // try to fix timestamp
+ return false;
+ }
+
+ if (status == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_DATA, 6, gyroData, "Failed to read BMX055 gyro data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, BMX055_ACCEL_X_LSB, 6, accelData, "Failed to read BMX055 accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_X_LSB, 8, magData, "Failed to read BMX055 mag data"))
+ return false;
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+
+ // need to prepare accel data
+
+ accelData[0] &= 0xf0;
+ accelData[2] &= 0xf0;
+ accelData[4] &= 0xf0;
+
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+
+ float mx, my, mz;
+
+ processMagData(magData, mx, my, mz);
+ m_imuData.compass.setX(mx);
+ m_imuData.compass.setY(my);
+ m_imuData.compass.setZ(mz);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel axes
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out mag axes
+
+#ifdef BMX055_REMAP
+ m_imuData.compass.setY(-m_imuData.compass.y());
+ m_imuData.compass.setZ(-m_imuData.compass.z());
+#else
+ RTFLOAT temp = m_imuData.compass.x();
+ m_imuData.compass.setX(-m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+ m_imuData.compass.setZ(-m_imuData.compass.z());
+#endif
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
+
+/*!
+ * @brief This API reads remapped compensated Magnetometer
+ * data of X,Y,Z values
+ * @note The output value of compensated X, Y, Z as float
+ *
+ * @note In this function X and Y axis is remapped
+ * @note X is read from the address 0x44 & 0x45
+ * @note Y is read from the address 0x42 & 0x43
+ * @note this API is only applicable for BMX055 sensor
+ * *
+ *
+*/
+
+/****************************************************/
+/**\name ARRAY PARAMETERS */
+/***************************************************/
+#define LSB_ZERO 0
+#define MSB_ONE 1
+#define LSB_TWO 2
+#define MSB_THREE 3
+#define LSB_FOUR 4
+#define MSB_FIVE 5
+#define LSB_SIX 6
+#define MSB_SEVEN 7
+
+/********************************************/
+/**\name BIT MASK, LENGTH AND POSITION OF REMAPPED DATA REGISTERS */
+/********************************************/
+/* Data X LSB Remapped Register only applicable for BMX055 */
+#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__POS 3
+#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__LEN 5
+#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__MSK 0xF8
+#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__REG\
+BMM050_BMX055_REMAPPED_DATAX_LSB
+
+/* Data Y LSB Remapped Register only applicable for BMX055 */
+#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__POS 3
+#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__LEN 5
+#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__MSK 0xF8
+#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__REG\
+BMM050_BMX055_REMAPPED_DATAY_LSB
+
+/********************************************/
+/**\name BIT MASK, LENGTH AND POSITION OF DATA REGISTERS */
+/********************************************/
+/* Data X LSB Register */
+#define BMM050_DATAX_LSB_VALUEX__POS 3
+#define BMM050_DATAX_LSB_VALUEX__LEN 5
+#define BMM050_DATAX_LSB_VALUEX__MSK 0xF8
+#define BMM050_DATAX_LSB_VALUEX__REG BMM050_DATAX_LSB
+
+/* Data X SelfTest Register */
+#define BMM050_DATAX_LSB_TESTX__POS 0
+#define BMM050_DATAX_LSB_TESTX__LEN 1
+#define BMM050_DATAX_LSB_TESTX__MSK 0x01
+#define BMM050_DATAX_LSB_TESTX__REG BMM050_DATAX_LSB
+
+/* Data Y LSB Register */
+#define BMM050_DATAY_LSB_VALUEY__POS 3
+#define BMM050_DATAY_LSB_VALUEY__LEN 5
+#define BMM050_DATAY_LSB_VALUEY__MSK 0xF8
+#define BMM050_DATAY_LSB_VALUEY__REG BMM050_DATAY_LSB
+
+/* Data Y SelfTest Register */
+#define BMM050_DATAY_LSB_TESTY__POS 0
+#define BMM050_DATAY_LSB_TESTY__LEN 1
+#define BMM050_DATAY_LSB_TESTY__MSK 0x01
+#define BMM050_DATAY_LSB_TESTY__REG BMM050_DATAY_LSB
+
+/* Data Z LSB Register */
+#define BMM050_DATAZ_LSB_VALUEZ__POS 1
+#define BMM050_DATAZ_LSB_VALUEZ__LEN 7
+#define BMM050_DATAZ_LSB_VALUEZ__MSK 0xFE
+#define BMM050_DATAZ_LSB_VALUEZ__REG BMM050_DATAZ_LSB
+
+/* Data Z SelfTest Register */
+#define BMM050_DATAZ_LSB_TESTZ__POS 0
+#define BMM050_DATAZ_LSB_TESTZ__LEN 1
+#define BMM050_DATAZ_LSB_TESTZ__MSK 0x01
+#define BMM050_DATAZ_LSB_TESTZ__REG BMM050_DATAZ_LSB
+
+/* Hall Resistance LSB Register */
+#define BMM050_R_LSB_VALUE__POS 2
+#define BMM050_R_LSB_VALUE__LEN 6
+#define BMM050_R_LSB_VALUE__MSK 0xFC
+#define BMM050_R_LSB_VALUE__REG BMM050_R_LSB
+
+#define BMM050_DATA_RDYSTAT__POS 0
+#define BMM050_DATA_RDYSTAT__LEN 1
+#define BMM050_DATA_RDYSTAT__MSK 0x01
+#define BMM050_DATA_RDYSTAT__REG BMM050_R_LSB
+
+/********************************************/
+/**\name GET AND SET BITSLICE FUNCTIONS */
+/********************************************/
+/* get bit slice */
+#define BMM050_GET_BITSLICE(regvar, bitname)\
+ ((regvar & bitname##__MSK) >> bitname##__POS)
+
+/* Set bit slice */
+#define BMM050_SET_BITSLICE(regvar, bitname, val)\
+ ((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/********************************************/
+/**\name BIT SHIFTING DEFINITIONS */
+/********************************************/
+/*Shifting Constants*/
+#define SHIFT_RIGHT_1_POSITION 1
+#define SHIFT_RIGHT_2_POSITION 2
+#define SHIFT_RIGHT_3_POSITION 3
+#define SHIFT_RIGHT_4_POSITION 4
+#define SHIFT_RIGHT_5_POSITION 5
+#define SHIFT_RIGHT_6_POSITION 6
+#define SHIFT_RIGHT_7_POSITION 7
+#define SHIFT_RIGHT_8_POSITION 8
+#define SHIFT_RIGHT_9_POSITION 9
+#define SHIFT_RIGHT_12_POSITION 12
+#define SHIFT_RIGHT_13_POSITION 13
+#define SHIFT_RIGHT_16_POSITION 16
+
+#define SHIFT_LEFT_1_POSITION 1
+#define SHIFT_LEFT_2_POSITION 2
+#define SHIFT_LEFT_3_POSITION 3
+#define SHIFT_LEFT_4_POSITION 4
+#define SHIFT_LEFT_5_POSITION 5
+#define SHIFT_LEFT_6_POSITION 6
+#define SHIFT_LEFT_7_POSITION 7
+#define SHIFT_LEFT_8_POSITION 8
+#define SHIFT_LEFT_14_POSITION 14
+#define SHIFT_LEFT_15_POSITION 15
+#define SHIFT_LEFT_16_POSITION 16
+
+/****************************************************/
+/**\name COMPENSATED FORMULA DEFINITIONS */
+/***************************************************/
+#define BMM050_HEX_FOUR_THOUSAND 0x4000
+#define BMM050_HEX_ONE_LACK 0x100000
+#define BMM050_HEX_A_ZERO 0xA0
+
+#define BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR 16384.0f
+#define BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA 268435456.0f
+#define BMM050_FLOAT_1_6_3_8_4_DATA 16384.0f
+#define BMM050_FLOAT_2_5_6_DATA 256.0f
+#define BMM050_FLOAT_1_6_0_DATA 160.0f
+#define BMM050_FLOAT_8_1_9_2_DATA 8192.0f
+#define BMM050_FLOAT_EIGHT_DATA 8.0f
+#define BMM050_FLOAT_SIXTEEN_DATA 16.0f
+#define BMM050_FLOAT_1_3_1_0_7_2_DATA 131072.0f
+#define BMM050_FLOAT_3_2_7_6_8_DATA 32768.0
+#define BMM050_FLOAT_4_DATA 4.0
+
+/********************************************/
+/**\name ENABLE/DISABLE DEFINITIONS */
+/********************************************/
+#define BMM050_ZERO_U8X 0
+#define BMM050_DISABLE 0
+#define BMM050_ENABLE 1
+#define BMM050_CHANNEL_DISABLE 1
+#define BMM050_CHANNEL_ENABLE 0
+#define BMM050_INTPIN_LATCH_ENABLE 1
+#define BMM050_INTPIN_LATCH_DISABLE 0
+#define BMM050_OFF 0
+#define BMM050_ON 1
+
+/********************************************/
+/**\name OVERFLOW DEFINITIONS */
+/********************************************/
+/* compensated output value returned if sensor had overflow */
+#define BMM050_OVERFLOW_OUTPUT -32768
+#define BMM050_OVERFLOW_OUTPUT_S32 ((int32_t)(-2147483647-1))
+#define BMM050_OVERFLOW_OUTPUT_FLOAT 0.0f
+#define BMM050_FLIP_OVERFLOW_ADCVAL -4096
+#define BMM050_HALL_OVERFLOW_ADCVAL -16384
+
+/********************************************/
+/**\name NUMERIC DEFINITIONS */
+/********************************************/
+#define C_BMM050_ZERO_U8X ((uint8_t)0)
+#define C_BMM050_ONE_U8X ((uint8_t)1)
+#define C_BMM050_TWO_U8X ((uint8_t)2)
+#define C_BMM050_FOUR_U8X ((uint8_t)4)
+#define C_BMM050_FIVE_U8X ((uint8_t)5)
+#define C_BMM050_EIGHT_U8X ((uint8_t)8)
+
+#define BMM0505_HEX_ZERO_ZERO 0x00
+/* Conversion factors*/
+#define BMM050_CONVFACTOR_LSB_UT 6
+
+/********************************************/
+/**\name BIT MASK, LENGTH AND POSITION OF TRIM REGISTER */
+/********************************************/
+/* Register 6D */
+#define BMM050_DIG_XYZ1_MSB__POS 0
+#define BMM050_DIG_XYZ1_MSB__LEN 7
+#define BMM050_DIG_XYZ1_MSB__MSK 0x7F
+#define BMM050_DIG_XYZ1_MSB__REG BMM050_DIG_XYZ1_MSB
+
+#ifdef BMX055_REMAP
+void RTIMUBMX055::processMagData(unsigned char *v_data_uint8_t, float& magX, float& magY, float& magZ)
+{
+ /* structure used to store the mag raw xyz and r data */
+ struct {
+ int16_t raw_data_x;
+ int16_t raw_data_y;
+ int16_t raw_data_z;
+ uint16_t raw_data_r;
+ } raw_data_xyz_t;
+
+ /* Reading data for Y axis */
+ v_data_uint8_t[LSB_ZERO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_ZERO],
+ BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY);
+ raw_data_xyz_t.raw_data_y = (int16_t)((((int32_t)
+ ((int8_t)v_data_uint8_t[MSB_ONE])) <<
+ SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_ZERO]);
+
+ /* Reading data for X axis */
+ v_data_uint8_t[LSB_TWO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_TWO],
+ BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX);
+ raw_data_xyz_t.raw_data_x = (int16_t)((((int32_t)
+ ((int8_t)v_data_uint8_t[MSB_THREE])) <<
+ SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_TWO]);
+ raw_data_xyz_t.raw_data_x = -raw_data_xyz_t.raw_data_x;
+
+ /* Reading data for Z axis */
+ v_data_uint8_t[LSB_FOUR] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_FOUR],
+ BMM050_DATAZ_LSB_VALUEZ);
+ raw_data_xyz_t.raw_data_z = (int16_t)((((int32_t)
+ ((int8_t)v_data_uint8_t[MSB_FIVE])) <<
+ SHIFT_LEFT_7_POSITION) | v_data_uint8_t[LSB_FOUR]);
+
+ /* Reading data for Resistance*/
+ v_data_uint8_t[LSB_SIX] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_SIX],
+ BMM050_R_LSB_VALUE);
+ raw_data_xyz_t.raw_data_r = (uint16_t)((((uint32_t)
+ v_data_uint8_t[MSB_SEVEN]) <<
+ SHIFT_LEFT_6_POSITION) | v_data_uint8_t[LSB_SIX]);
+
+ /* Compensation for X axis */
+ magX = bmm050_compensate_X_float(
+ raw_data_xyz_t.raw_data_x,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Compensation for Y axis */
+ magY = bmm050_compensate_Y_float(
+ raw_data_xyz_t.raw_data_y,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Compensation for Z axis */
+ magZ = bmm050_compensate_Z_float(
+ raw_data_xyz_t.raw_data_z,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Output raw resistance value */
+// mag_data->resistance = raw_data_xyz_t.raw_data_r;
+
+}
+#else
+void RTIMUBMX055::processMagData(unsigned char *v_data_u8, float& magX, float& magY, float& magZ)
+{
+ /* structure used to store the mag raw xyz and r data */
+ struct {
+ int16_t raw_data_x;
+ int16_t raw_data_y;
+ int16_t raw_data_z;
+ int16_t raw_data_r;
+ } raw_data_xyz_t;
+
+ /* Reading data for X axis */
+ v_data_u8[LSB_ZERO] = BMM050_GET_BITSLICE(v_data_u8[LSB_ZERO],
+ BMM050_DATAX_LSB_VALUEX);
+ raw_data_xyz_t.raw_data_x = (int16_t)((((int32_t)
+ ((int8_t)v_data_u8[MSB_ONE])) <<
+ SHIFT_LEFT_5_POSITION) | v_data_u8[LSB_ZERO]);
+
+ /* Reading data for Y axis */
+ v_data_u8[LSB_TWO] = BMM050_GET_BITSLICE(v_data_u8[LSB_TWO],
+ BMM050_DATAY_LSB_VALUEY);
+ raw_data_xyz_t.raw_data_y = (int16_t)((((int32_t)
+ ((int8_t)v_data_u8[MSB_THREE])) <<
+ SHIFT_LEFT_5_POSITION) | v_data_u8[LSB_TWO]);
+
+ /* Reading data for Z axis */
+ v_data_u8[LSB_FOUR] = BMM050_GET_BITSLICE(v_data_u8[LSB_FOUR],
+ BMM050_DATAZ_LSB_VALUEZ);
+ raw_data_xyz_t.raw_data_z = (int16_t)((((int32_t)
+ ((int8_t)v_data_u8[MSB_FIVE])) <<
+ SHIFT_LEFT_7_POSITION) | v_data_u8[LSB_FOUR]);
+
+ /* Reading data for Resistance*/
+ v_data_u8[LSB_SIX] = BMM050_GET_BITSLICE(v_data_u8[LSB_SIX],
+ BMM050_R_LSB_VALUE);
+ raw_data_xyz_t.raw_data_r = (uint16_t)((((uint32_t)
+ v_data_u8[MSB_SEVEN]) <<
+ SHIFT_LEFT_6_POSITION) | v_data_u8[LSB_SIX]);
+
+ /* Compensation for X axis */
+ magX = bmm050_compensate_X_float(
+ raw_data_xyz_t.raw_data_x,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Compensation for Y axis */
+ magY = bmm050_compensate_Y_float(
+ raw_data_xyz_t.raw_data_y,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Compensation for Z axis */
+ magZ = bmm050_compensate_Z_float(
+ raw_data_xyz_t.raw_data_z,
+ raw_data_xyz_t.raw_data_r);
+
+ /* Output raw resistance value */
+// mag_data->resistance = raw_data_xyz_t.raw_data_r;
+
+}
+#endif
+
+float RTIMUBMX055::bmm050_compensate_X_float(int16_t mag_data_x, uint16_t data_r)
+{
+ float inter_retval = BMM050_ZERO_U8X;
+ if (mag_data_x != BMM050_FLIP_OVERFLOW_ADCVAL /* no overflow */
+ ) {
+ if (data_r != C_BMM050_ZERO_U8X) {
+ inter_retval = ((((float)m_dig_xyz1)
+ * BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
+ /data_r)
+ - BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
+ } else {
+ inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
+ return inter_retval;
+ }
+ inter_retval = (((mag_data_x * ((((((float)m_dig_xy2) *
+ (inter_retval*inter_retval /
+ BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
+ inter_retval * ((float)m_dig_xy1)
+ / BMM050_FLOAT_1_6_3_8_4_DATA))
+ + BMM050_FLOAT_2_5_6_DATA) *
+ (((float)m_dig_x2) + BMM050_FLOAT_1_6_0_DATA)))
+ / BMM050_FLOAT_8_1_9_2_DATA)
+ + (((float)m_dig_x1) *
+ BMM050_FLOAT_EIGHT_DATA))/
+ BMM050_FLOAT_SIXTEEN_DATA;
+ } else {
+ inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
+ }
+ return inter_retval;
+}
+
+float RTIMUBMX055::bmm050_compensate_Y_float(int16_t mag_data_y, uint16_t data_r)
+{
+ float inter_retval = BMM050_ZERO_U8X;
+ if (mag_data_y != BMM050_FLIP_OVERFLOW_ADCVAL /* no overflow */
+ ) {
+ if (data_r != C_BMM050_ZERO_U8X) {
+ inter_retval = ((((float)m_dig_xyz1)
+ * BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
+ /data_r) - BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
+ } else {
+ inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
+ return inter_retval;
+ }
+ inter_retval = (((mag_data_y * ((((((float)m_dig_xy2) *
+ (inter_retval*inter_retval
+ / BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
+ inter_retval * ((float)m_dig_xy1)
+ / BMM050_FLOAT_1_6_3_8_4_DATA)) +
+ BMM050_FLOAT_2_5_6_DATA) *
+ (((float)m_dig_y2) + BMM050_FLOAT_1_6_0_DATA)))
+ / BMM050_FLOAT_8_1_9_2_DATA) +
+ (((float)m_dig_y1) * BMM050_FLOAT_EIGHT_DATA))
+ / BMM050_FLOAT_SIXTEEN_DATA;
+ } else {
+ /* overflow, set output to 0.0f */
+ inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
+ }
+ return inter_retval;
+}
+
+float RTIMUBMX055::bmm050_compensate_Z_float (int16_t mag_data_z, uint16_t data_r)
+{
+ float inter_retval = BMM050_ZERO_U8X;
+ /* no overflow */
+ if (mag_data_z != BMM050_HALL_OVERFLOW_ADCVAL) {
+ if ((m_dig_z2 != BMM050_ZERO_U8X)
+ && (m_dig_z1 != BMM050_ZERO_U8X)
+ && (data_r != BMM050_ZERO_U8X)) {
+ inter_retval = ((((((float)mag_data_z)-
+ ((float)m_dig_z4))*
+ BMM050_FLOAT_1_3_1_0_7_2_DATA)-
+ (((float)m_dig_z3)*(((float)data_r)
+ -((float)m_dig_xyz1))))
+ /((((float)m_dig_z2)+
+ ((float)m_dig_z1)*((float)data_r) /
+ BMM050_FLOAT_3_2_7_6_8_DATA)
+ * BMM050_FLOAT_4_DATA))
+ / BMM050_FLOAT_SIXTEEN_DATA;
+ }
+ } else {
+ /* overflow, set output to 0.0f */
+ inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
+ }
+ return inter_retval;
+}
+
+bool RTIMUBMX055::magInitTrimRegisters()
+{
+ unsigned char data[2];
+ data[0] = 0;
+ data[1] = 0;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X1, 1, (uint8_t *)&m_dig_x1, "Failed to read BMX055 mag trim x1"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y1, 1, (uint8_t *)&m_dig_y1, "Failed to read BMX055 mag trim y1"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X2, 1, (uint8_t *)&m_dig_x2, "Failed to read BMX055 mag trim x2"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y2, 1, (uint8_t *)&m_dig_y2, "Failed to read BMX055 mag trim y2"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY1, 1, &m_dig_xy1, "Failed to read BMX055 mag trim xy1"))
+ return false;
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY2, 1, (uint8_t *)&m_dig_xy2, "Failed to read BMX055 mag trim xy2"))
+ return false;
+
+ /* shorts can not be recast into (uint8_t*)
+ * due to possible mix up between trim data
+ * arrangement and memory arrangement */
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z1_LSB, 2, data, "Failed to read BMX055 mag trim z1"))
+ return false;
+
+ m_dig_z1 = (uint16_t)((((uint32_t)((uint8_t)
+ data[MSB_ONE])) <<
+ SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z2_LSB, 2, data, "Failed to read BMX055 mag trim z2"))
+ return false;
+
+ m_dig_z2 = (int16_t)((((int32_t)(
+ (int8_t)data[MSB_ONE])) <<
+ SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z3_LSB, 2, data, "Failed to read BMX055 mag trim z3"))
+ return false;
+
+ m_dig_z3 = (int16_t)((((int32_t)(
+ (int8_t)data[MSB_ONE])) <<
+ SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z4_LSB, 2, data, "Failed to read BMX055 mag trim z4"))
+ return false;
+
+ m_dig_z4 = (int16_t)((((int32_t)(
+ (int8_t)data[MSB_ONE])) <<
+ SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
+
+ if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XYZ1_LSB, 2, data, "Failed to read BMX055 mag trim xyz1"))
+ return false;
+
+ data[MSB_ONE] = BMM050_GET_BITSLICE(data[MSB_ONE],
+ BMM050_DIG_XYZ1_MSB);
+ m_dig_xyz1 = (uint16_t)((((uint32_t)
+ ((uint8_t)data[MSB_ONE])) <<
+ SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.h
new file mode 100644
index 0000000..d386fb2
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBMX055.h
@@ -0,0 +1,80 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUBMX055_H
+#define _RTIMUBMX055_H
+
+#include "RTIMU.h"
+
+class RTIMUBMX055 : public RTIMU
+{
+public:
+ RTIMUBMX055(RTIMUSettings *settings);
+ ~RTIMUBMX055();
+
+ virtual const char *IMUName() { return "BMX055"; }
+ virtual int IMUType() { return RTIMU_TYPE_BMX055; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroFSR();
+ bool setAccelSampleRate();
+ bool setAccelFSR();
+ bool magInitTrimRegisters();
+ bool setMagPreset();
+ void processMagData(unsigned char *v_data_uint8_t, float& magX, float& magY, float& magZ);
+ float bmm050_compensate_X_float(int16_t mag_data_x, uint16_t data_r);
+ float bmm050_compensate_Y_float(int16_t mag_data_y, uint16_t data_r);
+ float bmm050_compensate_Z_float(int16_t mag_data_z, uint16_t data_r);
+
+ unsigned char m_gyroSlaveAddr; // I2C address of gyro
+ unsigned char m_accelSlaveAddr; // I2C address of accel
+ unsigned char m_magSlaveAddr; // I2C address of mag
+
+ bool m_firstTime; // if first sample
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+
+ int8_t m_dig_x1;/**< trim x1 data */
+ int8_t m_dig_y1;/**< trim y1 data */
+
+ int8_t m_dig_x2;/**< trim x2 data */
+ int8_t m_dig_y2;/**< trim y2 data */
+
+ uint16_t m_dig_z1;/**< trim z1 data */
+ int16_t m_dig_z2;/**< trim z2 data */
+ int16_t m_dig_z3;/**< trim z3 data */
+ int16_t m_dig_z4;/**< trim z4 data */
+
+ uint8_t m_dig_xy1;/**< trim xy1 data */
+ int8_t m_dig_xy2;/**< trim xy2 data */
+
+ uint16_t m_dig_xyz1;/**< trim xyz1 data */
+};
+
+#endif // _RTIMUBMX055_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.cpp
new file mode 100644
index 0000000..407a618
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.cpp
@@ -0,0 +1,191 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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.
+
+// Based on the Adafruit BNO055 driver:
+
+/***************************************************************************
+ This is a library for the BNO055 orientation sensor
+ Designed specifically to work with the Adafruit BNO055 Breakout.
+ Pick one up today in the adafruit shop!
+ ------> http://www.adafruit.com/products
+ These sensors use I2C to communicate, 2 pins are required to interface.
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit andopen-source hardware by purchasing products
+ from Adafruit!
+ Written by KTOWN for Adafruit Industries.
+ MIT license, all text above must be included in any redistribution
+ ***************************************************************************/
+
+#include "RTIMUBNO055.h"
+#include "RTIMUSettings.h"
+
+RTIMUBNO055::RTIMUBNO055(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+}
+
+RTIMUBNO055::~RTIMUBNO055()
+{
+}
+
+bool RTIMUBNO055::IMUInit()
+{
+ unsigned char result;
+
+ m_slaveAddr = m_settings->m_I2CSlaveAddress;
+ m_lastReadTime = RTMath::currentUSecsSinceEpoch();
+
+ // set validity flags
+
+ m_imuData.fusionPoseValid = true;
+ m_imuData.fusionQPoseValid = true;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ if (!m_settings->HALRead(m_slaveAddr, BNO055_WHO_AM_I, 1, &result, "Failed to read BNO055 id"))
+ return false;
+
+ if (result != BNO055_ID) {
+ HAL_ERROR1("Incorrect IMU id %d", result);
+ return false;
+ }
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_CONFIG, "Failed to set BNO055 into config mode"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_SYS_TRIGGER, 0x20, "Failed to reset BNO055"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ while (1) {
+ if (!m_settings->HALRead(m_slaveAddr, BNO055_WHO_AM_I, 1, &result, ""))
+ continue;
+ if (result == BNO055_ID)
+ break;
+ m_settings->delayMs(50);
+ }
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_PWR_MODE, BNO055_PWR_MODE_NORMAL, "Failed to set BNO055 normal power mode"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_PAGE_ID, 0, "Failed to set BNO055 page 0"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_SYS_TRIGGER, 0x00, "Failed to start BNO055"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_UNIT_SEL, 0x87, "Failed to set BNO055 units"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_NDOF, "Failed to set BNO055 into 9-dof mode"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ HAL_INFO("BNO055 init complete\n");
+ return true;
+}
+
+int RTIMUBNO055::IMUGetPollInterval()
+{
+ return (7);
+}
+
+bool RTIMUBNO055::IMURead()
+{
+ unsigned char buffer[24];
+
+ if ((RTMath::currentUSecsSinceEpoch() - m_lastReadTime) < m_sampleInterval)
+ return false; // too soon
+
+ m_lastReadTime = RTMath::currentUSecsSinceEpoch();
+ if (!m_settings->HALRead(m_slaveAddr, BNO055_ACCEL_DATA, 24, buffer, "Failed to read BNO055 data"))
+ return false;
+
+ int16_t x, y, z;
+
+ // process accel data
+
+ x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
+ y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
+ z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
+
+ m_imuData.accel.setX((RTFLOAT)y / 1000.0);
+ m_imuData.accel.setY((RTFLOAT)x / 1000.0);
+ m_imuData.accel.setZ((RTFLOAT)z / 1000.0);
+
+ // process mag data
+
+ x = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
+ y = (((uint16_t)buffer[9]) << 8) | ((uint16_t)buffer[8]);
+ z = (((uint16_t)buffer[11]) << 8) | ((uint16_t)buffer[10]);
+
+ m_imuData.compass.setX(-(RTFLOAT)y / 16.0);
+ m_imuData.compass.setY(-(RTFLOAT)x / 16.0);
+ m_imuData.compass.setZ(-(RTFLOAT)z / 16.0);
+
+ // process gyro data
+
+ x = (((uint16_t)buffer[13]) << 8) | ((uint16_t)buffer[12]);
+ y = (((uint16_t)buffer[15]) << 8) | ((uint16_t)buffer[14]);
+ z = (((uint16_t)buffer[17]) << 8) | ((uint16_t)buffer[16]);
+
+ m_imuData.gyro.setX(-(RTFLOAT)y / 900.0);
+ m_imuData.gyro.setY(-(RTFLOAT)x / 900.0);
+ m_imuData.gyro.setZ(-(RTFLOAT)z / 900.0);
+
+ // process euler angles
+
+ x = (((uint16_t)buffer[19]) << 8) | ((uint16_t)buffer[18]);
+ y = (((uint16_t)buffer[21]) << 8) | ((uint16_t)buffer[20]);
+ z = (((uint16_t)buffer[23]) << 8) | ((uint16_t)buffer[22]);
+
+ // put in structure and do axis remap
+
+ m_imuData.fusionPose.setX((RTFLOAT)y / 900.0);
+ m_imuData.fusionPose.setY((RTFLOAT)z / 900.0);
+ m_imuData.fusionPose.setZ((RTFLOAT)x / 900.0);
+
+ m_imuData.fusionQPose.fromEuler(m_imuData.fusionPose);
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ return true;
+}
[6/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
Posted by al...@apache.org.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
new file mode 100644
index 0000000..967cdaf
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
@@ -0,0 +1,48 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUBNO055_H
+#define _RTIMUBNO055_H
+
+#include "RTIMU.h"
+
+class RTIMUBNO055 : public RTIMU
+{
+public:
+ RTIMUBNO055(RTIMUSettings *settings);
+ ~RTIMUBNO055();
+
+ virtual const char *IMUName() { return "BNO055"; }
+ virtual int IMUType() { return RTIMU_TYPE_BNO055; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ unsigned char m_slaveAddr; // I2C address of BNO055
+
+ uint64_t m_lastReadTime;
+};
+
+#endif // _RTIMUBNO055_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
new file mode 100644
index 0000000..f0ba1f4
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
@@ -0,0 +1,1119 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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
+// staslock@gmail.com (www.clickdrive.io)
+
+
+#ifndef _RTIMUDEFS_H
+#define _RTIMUDEFS_H
+
+// IMU type codes
+//
+// For compatibility, only add new codes at the end to avoid renumbering
+
+#define RTIMU_TYPE_AUTODISCOVER 0 // audodiscover the IMU
+#define RTIMU_TYPE_NULL 1 // if no physical hardware
+#define RTIMU_TYPE_MPU9150 2 // InvenSense MPU9150
+#define RTIMU_TYPE_GD20HM303D 3 // STM L3GD20H/LSM303D (Pololu Altimu)
+#define RTIMU_TYPE_GD20M303DLHC 4 // STM L3GD20/LSM303DHLC (old Adafruit IMU)
+#define RTIMU_TYPE_LSM9DS0 5 // STM LSM9DS0 (eg Sparkfun IMU)
+#define RTIMU_TYPE_LSM9DS1 6 // STM LSM9DS1
+#define RTIMU_TYPE_MPU9250 7 // InvenSense MPU9250
+#define RTIMU_TYPE_GD20HM303DLHC 8 // STM L3GD20H/LSM303DHLC (new Adafruit IMU)
+#define RTIMU_TYPE_BMX055 9 // Bosch BMX055
+#define RTIMU_TYPE_BNO055 10 // Bosch BNO055
+
+//----------------------------------------------------------
+//
+// MPU-9150
+
+// MPU9150 I2C Slave Addresses
+
+#define MPU9150_ADDRESS0 0x68
+#define MPU9150_ADDRESS1 0x69
+#define MPU9150_ID 0x68
+
+// thes magnetometers are on aux bus
+
+#define AK8975_ADDRESS 0x0c
+#define HMC5883_ADDRESS 0x1e
+
+// Register map
+
+#define MPU9150_YG_OFFS_TC 0x01
+#define MPU9150_SMPRT_DIV 0x19
+#define MPU9150_LPF_CONFIG 0x1a
+#define MPU9150_GYRO_CONFIG 0x1b
+#define MPU9150_ACCEL_CONFIG 0x1c
+#define MPU9150_FIFO_EN 0x23
+#define MPU9150_I2C_MST_CTRL 0x24
+#define MPU9150_I2C_SLV0_ADDR 0x25
+#define MPU9150_I2C_SLV0_REG 0x26
+#define MPU9150_I2C_SLV0_CTRL 0x27
+#define MPU9150_I2C_SLV1_ADDR 0x28
+#define MPU9150_I2C_SLV1_REG 0x29
+#define MPU9150_I2C_SLV1_CTRL 0x2a
+#define MPU9150_I2C_SLV4_CTRL 0x34
+#define MPU9150_INT_PIN_CFG 0x37
+#define MPU9150_INT_ENABLE 0x38
+#define MPU9150_INT_STATUS 0x3a
+#define MPU9150_ACCEL_XOUT_H 0x3b
+#define MPU9150_GYRO_XOUT_H 0x43
+#define MPU9150_EXT_SENS_DATA_00 0x49
+#define MPU9150_I2C_SLV1_DO 0x64
+#define MPU9150_I2C_MST_DELAY_CTRL 0x67
+#define MPU9150_USER_CTRL 0x6a
+#define MPU9150_PWR_MGMT_1 0x6b
+#define MPU9150_PWR_MGMT_2 0x6c
+#define MPU9150_FIFO_COUNT_H 0x72
+#define MPU9150_FIFO_R_W 0x74
+#define MPU9150_WHO_AM_I 0x75
+
+// sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9150_SAMPLERATE_MIN 5 // 5 samples per second is the lowest
+#define MPU9150_SAMPLERATE_MAX 1000 // 1000 samples per second is the absolute maximum
+
+// compass rate defines
+
+#define MPU9150_COMPASSRATE_MIN 1 // 1 samples per second is the lowest
+#define MPU9150_COMPASSRATE_MAX 100 // 100 samples per second is maximum
+
+// LPF options (gyros and accels)
+
+#define MPU9150_LPF_256 0 // gyro: 256Hz, accel: 260Hz
+#define MPU9150_LPF_188 1 // gyro: 188Hz, accel: 184Hz
+#define MPU9150_LPF_98 2 // gyro: 98Hz, accel: 98Hz
+#define MPU9150_LPF_42 3 // gyro: 42Hz, accel: 44Hz
+#define MPU9150_LPF_20 4 // gyro: 20Hz, accel: 21Hz
+#define MPU9150_LPF_10 5 // gyro: 10Hz, accel: 10Hz
+#define MPU9150_LPF_5 6 // gyro: 5Hz, accel: 5Hz
+
+// Gyro FSR options
+
+#define MPU9150_GYROFSR_250 0 // +/- 250 degrees per second
+#define MPU9150_GYROFSR_500 8 // +/- 500 degrees per second
+#define MPU9150_GYROFSR_1000 0x10 // +/- 1000 degrees per second
+#define MPU9150_GYROFSR_2000 0x18 // +/- 2000 degrees per second
+
+// Accel FSR options
+
+#define MPU9150_ACCELFSR_2 0 // +/- 2g
+#define MPU9150_ACCELFSR_4 8 // +/- 4g
+#define MPU9150_ACCELFSR_8 0x10 // +/- 8g
+#define MPU9150_ACCELFSR_16 0x18 // +/- 16g
+
+
+// AK8975 compass registers
+
+#define AK8975_DEVICEID 0x0 // the device ID
+#define AK8975_ST1 0x02 // status 1
+#define AK8975_CNTL 0x0a // control reg
+#define AK8975_ASAX 0x10 // start of the fuse ROM data
+
+// HMC5883 compass registers
+
+#define HMC5883_CONFIG_A 0x00 // configuration A
+#define HMC5883_CONFIG_B 0x01 // configuration B
+#define HMC5883_MODE 0x02 // mode
+#define HMC5883_DATA_X_HI 0x03 // data x msb
+#define HMC5883_STATUS 0x09 // status
+#define HMC5883_ID 0x0a // id
+
+
+//----------------------------------------------------------
+//
+// MPU-9250
+
+// MPU9250 I2C Slave Addresses
+
+#define MPU9250_ADDRESS0 0x68
+#define MPU9250_ADDRESS1 0x69
+#define MPU9250_ID 0x71
+
+#define AK8963_ADDRESS 0x0c
+
+// Register map
+
+#define MPU9250_SMPRT_DIV 0x19
+#define MPU9250_GYRO_LPF 0x1a
+#define MPU9250_GYRO_CONFIG 0x1b
+#define MPU9250_ACCEL_CONFIG 0x1c
+#define MPU9250_ACCEL_LPF 0x1d
+#define MPU9250_FIFO_EN 0x23
+#define MPU9250_I2C_MST_CTRL 0x24
+#define MPU9250_I2C_SLV0_ADDR 0x25
+#define MPU9250_I2C_SLV0_REG 0x26
+#define MPU9250_I2C_SLV0_CTRL 0x27
+#define MPU9250_I2C_SLV1_ADDR 0x28
+#define MPU9250_I2C_SLV1_REG 0x29
+#define MPU9250_I2C_SLV1_CTRL 0x2a
+#define MPU9250_I2C_SLV2_ADDR 0x2b
+#define MPU9250_I2C_SLV2_REG 0x2c
+#define MPU9250_I2C_SLV2_CTRL 0x2d
+#define MPU9250_I2C_SLV4_CTRL 0x34
+#define MPU9250_INT_PIN_CFG 0x37
+#define MPU9250_INT_ENABLE 0x38
+#define MPU9250_INT_STATUS 0x3a
+#define MPU9250_ACCEL_XOUT_H 0x3b
+#define MPU9250_GYRO_XOUT_H 0x43
+#define MPU9250_EXT_SENS_DATA_00 0x49
+#define MPU9250_I2C_SLV1_DO 0x64
+#define MPU9250_I2C_MST_DELAY_CTRL 0x67
+#define MPU9250_USER_CTRL 0x6a
+#define MPU9250_PWR_MGMT_1 0x6b
+#define MPU9250_PWR_MGMT_2 0x6c
+#define MPU9250_FIFO_COUNT_H 0x72
+#define MPU9250_FIFO_R_W 0x74
+#define MPU9250_WHO_AM_I 0x75
+
+// sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9250_SAMPLERATE_MIN 5 // 5 samples per second is the lowest
+#define MPU9250_SAMPLERATE_MAX 32000 // 32000 samples per second is the absolute maximum
+
+// compass rate defines
+
+#define MPU9250_COMPASSRATE_MIN 1 // 1 samples per second is the lowest
+#define MPU9250_COMPASSRATE_MAX 100 // 100 samples per second is maximum
+
+// Gyro LPF options
+
+#define MPU9250_GYRO_LPF_8800 0x11 // 8800Hz, 0.64mS delay
+#define MPU9250_GYRO_LPF_3600 0x10 // 3600Hz, 0.11mS delay
+#define MPU9250_GYRO_LPF_250 0x00 // 250Hz, 0.97mS delay
+#define MPU9250_GYRO_LPF_184 0x01 // 184Hz, 2.9mS delay
+#define MPU9250_GYRO_LPF_92 0x02 // 92Hz, 3.9mS delay
+#define MPU9250_GYRO_LPF_41 0x03 // 41Hz, 5.9mS delay
+#define MPU9250_GYRO_LPF_20 0x04 // 20Hz, 9.9mS delay
+#define MPU9250_GYRO_LPF_10 0x05 // 10Hz, 17.85mS delay
+#define MPU9250_GYRO_LPF_5 0x06 // 5Hz, 33.48mS delay
+
+// Gyro FSR options
+
+#define MPU9250_GYROFSR_250 0 // +/- 250 degrees per second
+#define MPU9250_GYROFSR_500 8 // +/- 500 degrees per second
+#define MPU9250_GYROFSR_1000 0x10 // +/- 1000 degrees per second
+#define MPU9250_GYROFSR_2000 0x18 // +/- 2000 degrees per second
+
+// Accel FSR options
+
+#define MPU9250_ACCELFSR_2 0 // +/- 2g
+#define MPU9250_ACCELFSR_4 8 // +/- 4g
+#define MPU9250_ACCELFSR_8 0x10 // +/- 8g
+#define MPU9250_ACCELFSR_16 0x18 // +/- 16g
+
+// Accel LPF options
+
+#define MPU9250_ACCEL_LPF_1130 0x08 // 1130Hz, 0.75mS delay
+#define MPU9250_ACCEL_LPF_460 0x00 // 460Hz, 1.94mS delay
+#define MPU9250_ACCEL_LPF_184 0x01 // 184Hz, 5.80mS delay
+#define MPU9250_ACCEL_LPF_92 0x02 // 92Hz, 7.80mS delay
+#define MPU9250_ACCEL_LPF_41 0x03 // 41Hz, 11.80mS delay
+#define MPU9250_ACCEL_LPF_20 0x04 // 20Hz, 19.80mS delay
+#define MPU9250_ACCEL_LPF_10 0x05 // 10Hz, 35.70mS delay
+#define MPU9250_ACCEL_LPF_5 0x06 // 5Hz, 66.96mS delay
+
+// AK8963 compass registers
+
+#define AK8963_DEVICEID 0x48 // the device ID
+#define AK8963_ST1 0x02 // status 1
+#define AK8963_CNTL 0x0a // control reg
+#define AK8963_ASAX 0x10 // start of the fuse ROM data
+
+//----------------------------------------------------------
+//
+// L3GD20
+
+// I2C Slave Addresses
+
+#define L3GD20_ADDRESS0 0x6a
+#define L3GD20_ADDRESS1 0x6b
+#define L3GD20_ID 0xd4
+
+// L3GD20 Register map
+
+#define L3GD20_WHO_AM_I 0x0f
+#define L3GD20_CTRL1 0x20
+#define L3GD20_CTRL2 0x21
+#define L3GD20_CTRL3 0x22
+#define L3GD20_CTRL4 0x23
+#define L3GD20_CTRL5 0x24
+#define L3GD20_OUT_TEMP 0x26
+#define L3GD20_STATUS 0x27
+#define L3GD20_OUT_X_L 0x28
+#define L3GD20_OUT_X_H 0x29
+#define L3GD20_OUT_Y_L 0x2a
+#define L3GD20_OUT_Y_H 0x2b
+#define L3GD20_OUT_Z_L 0x2c
+#define L3GD20_OUT_Z_H 0x2d
+#define L3GD20_FIFO_CTRL 0x2e
+#define L3GD20_FIFO_SRC 0x2f
+#define L3GD20_IG_CFG 0x30
+#define L3GD20_IG_SRC 0x31
+#define L3GD20_IG_THS_XH 0x32
+#define L3GD20_IG_THS_XL 0x33
+#define L3GD20_IG_THS_YH 0x34
+#define L3GD20_IG_THS_YL 0x35
+#define L3GD20_IG_THS_ZH 0x36
+#define L3GD20_IG_THS_ZL 0x37
+#define L3GD20_IG_DURATION 0x38
+
+// Gyro sample rate defines
+
+#define L3GD20_SAMPLERATE_95 0
+#define L3GD20_SAMPLERATE_190 1
+#define L3GD20_SAMPLERATE_380 2
+#define L3GD20_SAMPLERATE_760 3
+
+// Gyro banwidth defines
+
+#define L3GD20_BANDWIDTH_0 0
+#define L3GD20_BANDWIDTH_1 1
+#define L3GD20_BANDWIDTH_2 2
+#define L3GD20_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define L3GD20_FSR_250 0
+#define L3GD20_FSR_500 1
+#define L3GD20_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define L3GD20_HPF_0 0
+#define L3GD20_HPF_1 1
+#define L3GD20_HPF_2 2
+#define L3GD20_HPF_3 3
+#define L3GD20_HPF_4 4
+#define L3GD20_HPF_5 5
+#define L3GD20_HPF_6 6
+#define L3GD20_HPF_7 7
+#define L3GD20_HPF_8 8
+#define L3GD20_HPF_9 9
+
+//----------------------------------------------------------
+//
+// L3GD20H
+
+// I2C Slave Addresses
+
+#define L3GD20H_ADDRESS0 0x6a
+#define L3GD20H_ADDRESS1 0x6b
+#define L3GD20H_ID 0xd7
+
+// L3GD20H Register map
+
+#define L3GD20H_WHO_AM_I 0x0f
+#define L3GD20H_CTRL1 0x20
+#define L3GD20H_CTRL2 0x21
+#define L3GD20H_CTRL3 0x22
+#define L3GD20H_CTRL4 0x23
+#define L3GD20H_CTRL5 0x24
+#define L3GD20H_OUT_TEMP 0x26
+#define L3GD20H_STATUS 0x27
+#define L3GD20H_OUT_X_L 0x28
+#define L3GD20H_OUT_X_H 0x29
+#define L3GD20H_OUT_Y_L 0x2a
+#define L3GD20H_OUT_Y_H 0x2b
+#define L3GD20H_OUT_Z_L 0x2c
+#define L3GD20H_OUT_Z_H 0x2d
+#define L3GD20H_FIFO_CTRL 0x2e
+#define L3GD20H_FIFO_SRC 0x2f
+#define L3GD20H_IG_CFG 0x30
+#define L3GD20H_IG_SRC 0x31
+#define L3GD20H_IG_THS_XH 0x32
+#define L3GD20H_IG_THS_XL 0x33
+#define L3GD20H_IG_THS_YH 0x34
+#define L3GD20H_IG_THS_YL 0x35
+#define L3GD20H_IG_THS_ZH 0x36
+#define L3GD20H_IG_THS_ZL 0x37
+#define L3GD20H_IG_DURATION 0x38
+#define L3GD20H_LOW_ODR 0x39
+
+// Gyro sample rate defines
+
+#define L3GD20H_SAMPLERATE_12_5 0
+#define L3GD20H_SAMPLERATE_25 1
+#define L3GD20H_SAMPLERATE_50 2
+#define L3GD20H_SAMPLERATE_100 3
+#define L3GD20H_SAMPLERATE_200 4
+#define L3GD20H_SAMPLERATE_400 5
+#define L3GD20H_SAMPLERATE_800 6
+
+// Gyro banwidth defines
+
+#define L3GD20H_BANDWIDTH_0 0
+#define L3GD20H_BANDWIDTH_1 1
+#define L3GD20H_BANDWIDTH_2 2
+#define L3GD20H_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define L3GD20H_FSR_245 0
+#define L3GD20H_FSR_500 1
+#define L3GD20H_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define L3GD20H_HPF_0 0
+#define L3GD20H_HPF_1 1
+#define L3GD20H_HPF_2 2
+#define L3GD20H_HPF_3 3
+#define L3GD20H_HPF_4 4
+#define L3GD20H_HPF_5 5
+#define L3GD20H_HPF_6 6
+#define L3GD20H_HPF_7 7
+#define L3GD20H_HPF_8 8
+#define L3GD20H_HPF_9 9
+
+//----------------------------------------------------------
+//
+// LSM303D
+
+#define LSM303D_ADDRESS0 0x1e
+#define LSM303D_ADDRESS1 0x1d
+#define LSM303D_ID 0x49
+
+// LSM303D Register Map
+
+#define LSM303D_TEMP_OUT_L 0x05
+#define LSM303D_TEMP_OUT_H 0x06
+#define LSM303D_STATUS_M 0x07
+#define LSM303D_OUT_X_L_M 0x08
+#define LSM303D_OUT_X_H_M 0x09
+#define LSM303D_OUT_Y_L_M 0x0a
+#define LSM303D_OUT_Y_H_M 0x0b
+#define LSM303D_OUT_Z_L_M 0x0c
+#define LSM303D_OUT_Z_H_M 0x0d
+#define LSM303D_WHO_AM_I 0x0f
+#define LSM303D_INT_CTRL_M 0x12
+#define LSM303D_INT_SRC_M 0x13
+#define LSM303D_INT_THS_L_M 0x14
+#define LSM303D_INT_THS_H_M 0x15
+#define LSM303D_OFFSET_X_L_M 0x16
+#define LSM303D_OFFSET_X_H_M 0x17
+#define LSM303D_OFFSET_Y_L_M 0x18
+#define LSM303D_OFFSET_Y_H_M 0x19
+#define LSM303D_OFFSET_Z_L_M 0x1a
+#define LSM303D_OFFSET_Z_H_M 0x1b
+#define LSM303D_REFERENCE_X 0x1c
+#define LSM303D_REFERENCE_Y 0x1d
+#define LSM303D_REFERENCE_Z 0x1e
+#define LSM303D_CTRL0 0x1f
+#define LSM303D_CTRL1 0x20
+#define LSM303D_CTRL2 0x21
+#define LSM303D_CTRL3 0x22
+#define LSM303D_CTRL4 0x23
+#define LSM303D_CTRL5 0x24
+#define LSM303D_CTRL6 0x25
+#define LSM303D_CTRL7 0x26
+#define LSM303D_STATUS_A 0x27
+#define LSM303D_OUT_X_L_A 0x28
+#define LSM303D_OUT_X_H_A 0x29
+#define LSM303D_OUT_Y_L_A 0x2a
+#define LSM303D_OUT_Y_H_A 0x2b
+#define LSM303D_OUT_Z_L_A 0x2c
+#define LSM303D_OUT_Z_H_A 0x2d
+#define LSM303D_FIFO_CTRL 0x2e
+#define LSM303D_FIFO_SRC 0x2f
+#define LSM303D_IG_CFG1 0x30
+#define LSM303D_IG_SRC1 0x31
+#define LSM303D_IG_THS1 0x32
+#define LSM303D_IG_DUR1 0x33
+#define LSM303D_IG_CFG2 0x34
+#define LSM303D_IG_SRC2 0x35
+#define LSM303D_IG_THS2 0x36
+#define LSM303D_IG_DUR2 0x37
+#define LSM303D_CLICK_CFG 0x38
+#define LSM303D_CLICK_SRC 0x39
+#define LSM303D_CLICK_THS 0x3a
+#define LSM303D_TIME_LIMIT 0x3b
+#define LSM303D_TIME_LATENCY 0x3c
+#define LSM303D_TIME_WINDOW 0x3d
+#define LSM303D_ACT_THIS 0x3e
+#define LSM303D_ACT_DUR 0x3f
+
+// Accel sample rate defines
+
+#define LSM303D_ACCEL_SAMPLERATE_3_125 1
+#define LSM303D_ACCEL_SAMPLERATE_6_25 2
+#define LSM303D_ACCEL_SAMPLERATE_12_5 3
+#define LSM303D_ACCEL_SAMPLERATE_25 4
+#define LSM303D_ACCEL_SAMPLERATE_50 5
+#define LSM303D_ACCEL_SAMPLERATE_100 6
+#define LSM303D_ACCEL_SAMPLERATE_200 7
+#define LSM303D_ACCEL_SAMPLERATE_400 8
+#define LSM303D_ACCEL_SAMPLERATE_800 9
+#define LSM303D_ACCEL_SAMPLERATE_1600 10
+
+// Accel FSR
+
+#define LSM303D_ACCEL_FSR_2 0
+#define LSM303D_ACCEL_FSR_4 1
+#define LSM303D_ACCEL_FSR_6 2
+#define LSM303D_ACCEL_FSR_8 3
+#define LSM303D_ACCEL_FSR_16 4
+
+// Accel filter bandwidth
+
+#define LSM303D_ACCEL_LPF_773 0
+#define LSM303D_ACCEL_LPF_194 1
+#define LSM303D_ACCEL_LPF_362 2
+#define LSM303D_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM303D_COMPASS_SAMPLERATE_3_125 0
+#define LSM303D_COMPASS_SAMPLERATE_6_25 1
+#define LSM303D_COMPASS_SAMPLERATE_12_5 2
+#define LSM303D_COMPASS_SAMPLERATE_25 3
+#define LSM303D_COMPASS_SAMPLERATE_50 4
+#define LSM303D_COMPASS_SAMPLERATE_100 5
+
+// Compass FSR
+
+#define LSM303D_COMPASS_FSR_2 0
+#define LSM303D_COMPASS_FSR_4 1
+#define LSM303D_COMPASS_FSR_8 2
+#define LSM303D_COMPASS_FSR_12 3
+
+//----------------------------------------------------------
+//
+// LSM303DLHC
+
+#define LSM303DLHC_ACCEL_ADDRESS 0x19
+#define LSM303DLHC_COMPASS_ADDRESS 0x1e
+
+// LSM303DLHC Accel Register Map
+
+#define LSM303DLHC_CTRL1_A 0x20
+#define LSM303DLHC_CTRL2_A 0x21
+#define LSM303DLHC_CTRL3_A 0x22
+#define LSM303DLHC_CTRL4_A 0x23
+#define LSM303DLHC_CTRL5_A 0x24
+#define LSM303DLHC_CTRL6_A 0x25
+#define LSM303DLHC_REF_A 0x26
+#define LSM303DLHC_STATUS_A 0x27
+#define LSM303DLHC_OUT_X_L_A 0x28
+#define LSM303DLHC_OUT_X_H_A 0x29
+#define LSM303DLHC_OUT_Y_L_A 0x2a
+#define LSM303DLHC_OUT_Y_H_A 0x2b
+#define LSM303DLHC_OUT_Z_L_A 0x2c
+#define LSM303DLHC_OUT_Z_H_A 0x2d
+#define LSM303DLHC_FIFO_CTRL_A 0x2e
+#define LSM303DLHC_FIFO_SRC_A 0x2f
+
+// LSM303DLHC Compass Register Map
+
+#define LSM303DLHC_CRA_M 0x00
+#define LSM303DLHC_CRB_M 0x01
+#define LSM303DLHC_CRM_M 0x02
+#define LSM303DLHC_OUT_X_H_M 0x03
+#define LSM303DLHC_OUT_X_L_M 0x04
+#define LSM303DLHC_OUT_Y_H_M 0x05
+#define LSM303DLHC_OUT_Y_L_M 0x06
+#define LSM303DLHC_OUT_Z_H_M 0x07
+#define LSM303DLHC_OUT_Z_L_M 0x08
+#define LSM303DLHC_STATUS_M 0x09
+#define LSM303DLHC_TEMP_OUT_L_M 0x31
+#define LSM303DLHC_TEMP_OUT_H_M 0x32
+
+// Accel sample rate defines
+
+#define LSM303DLHC_ACCEL_SAMPLERATE_1 1
+#define LSM303DLHC_ACCEL_SAMPLERATE_10 2
+#define LSM303DLHC_ACCEL_SAMPLERATE_25 3
+#define LSM303DLHC_ACCEL_SAMPLERATE_50 4
+#define LSM303DLHC_ACCEL_SAMPLERATE_100 5
+#define LSM303DLHC_ACCEL_SAMPLERATE_200 6
+#define LSM303DLHC_ACCEL_SAMPLERATE_400 7
+
+// Accel FSR
+
+#define LSM303DLHC_ACCEL_FSR_2 0
+#define LSM303DLHC_ACCEL_FSR_4 1
+#define LSM303DLHC_ACCEL_FSR_8 2
+#define LSM303DLHC_ACCEL_FSR_16 3
+
+// Compass sample rate defines
+
+#define LSM303DLHC_COMPASS_SAMPLERATE_0_75 0
+#define LSM303DLHC_COMPASS_SAMPLERATE_1_5 1
+#define LSM303DLHC_COMPASS_SAMPLERATE_3 2
+#define LSM303DLHC_COMPASS_SAMPLERATE_7_5 3
+#define LSM303DLHC_COMPASS_SAMPLERATE_15 4
+#define LSM303DLHC_COMPASS_SAMPLERATE_30 5
+#define LSM303DLHC_COMPASS_SAMPLERATE_75 6
+#define LSM303DLHC_COMPASS_SAMPLERATE_220 7
+
+// Compass FSR
+
+#define LSM303DLHC_COMPASS_FSR_1_3 1
+#define LSM303DLHC_COMPASS_FSR_1_9 2
+#define LSM303DLHC_COMPASS_FSR_2_5 3
+#define LSM303DLHC_COMPASS_FSR_4 4
+#define LSM303DLHC_COMPASS_FSR_4_7 5
+#define LSM303DLHC_COMPASS_FSR_5_6 6
+#define LSM303DLHC_COMPASS_FSR_8_1 7
+
+//----------------------------------------------------------
+//
+// LSM9DS0
+
+// I2C Slave Addresses
+
+#define LSM9DS0_GYRO_ADDRESS0 0x6a
+#define LSM9DS0_GYRO_ADDRESS1 0x6b
+#define LSM9DS0_GYRO_ID 0xd4
+
+#define LSM9DS0_ACCELMAG_ADDRESS0 0x1e
+#define LSM9DS0_ACCELMAG_ADDRESS1 0x1d
+#define LSM9DS0_ACCELMAG_ID 0x49
+
+// LSM9DS0 Register map
+
+#define LSM9DS0_GYRO_WHO_AM_I 0x0f
+#define LSM9DS0_GYRO_CTRL1 0x20
+#define LSM9DS0_GYRO_CTRL2 0x21
+#define LSM9DS0_GYRO_CTRL3 0x22
+#define LSM9DS0_GYRO_CTRL4 0x23
+#define LSM9DS0_GYRO_CTRL5 0x24
+#define LSM9DS0_GYRO_OUT_TEMP 0x26
+#define LSM9DS0_GYRO_STATUS 0x27
+#define LSM9DS0_GYRO_OUT_X_L 0x28
+#define LSM9DS0_GYRO_OUT_X_H 0x29
+#define LSM9DS0_GYRO_OUT_Y_L 0x2a
+#define LSM9DS0_GYRO_OUT_Y_H 0x2b
+#define LSM9DS0_GYRO_OUT_Z_L 0x2c
+#define LSM9DS0_GYRO_OUT_Z_H 0x2d
+#define LSM9DS0_GYRO_FIFO_CTRL 0x2e
+#define LSM9DS0_GYRO_FIFO_SRC 0x2f
+#define LSM9DS0_GYRO_IG_CFG 0x30
+#define LSM9DS0_GYRO_IG_SRC 0x31
+#define LSM9DS0_GYRO_IG_THS_XH 0x32
+#define LSM9DS0_GYRO_IG_THS_XL 0x33
+#define LSM9DS0_GYRO_IG_THS_YH 0x34
+#define LSM9DS0_GYRO_IG_THS_YL 0x35
+#define LSM9DS0_GYRO_IG_THS_ZH 0x36
+#define LSM9DS0_GYRO_IG_THS_ZL 0x37
+#define LSM9DS0_GYRO_IG_DURATION 0x38
+
+// Gyro sample rate defines
+
+#define LSM9DS0_GYRO_SAMPLERATE_95 0
+#define LSM9DS0_GYRO_SAMPLERATE_190 1
+#define LSM9DS0_GYRO_SAMPLERATE_380 2
+#define LSM9DS0_GYRO_SAMPLERATE_760 3
+
+// Gyro banwidth defines
+
+#define LSM9DS0_GYRO_BANDWIDTH_0 0
+#define LSM9DS0_GYRO_BANDWIDTH_1 1
+#define LSM9DS0_GYRO_BANDWIDTH_2 2
+#define LSM9DS0_GYRO_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define LSM9DS0_GYRO_FSR_250 0
+#define LSM9DS0_GYRO_FSR_500 1
+#define LSM9DS0_GYRO_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define LSM9DS0_GYRO_HPF_0 0
+#define LSM9DS0_GYRO_HPF_1 1
+#define LSM9DS0_GYRO_HPF_2 2
+#define LSM9DS0_GYRO_HPF_3 3
+#define LSM9DS0_GYRO_HPF_4 4
+#define LSM9DS0_GYRO_HPF_5 5
+#define LSM9DS0_GYRO_HPF_6 6
+#define LSM9DS0_GYRO_HPF_7 7
+#define LSM9DS0_GYRO_HPF_8 8
+#define LSM9DS0_GYRO_HPF_9 9
+
+// Accel/Mag Register Map
+
+#define LSM9DS0_TEMP_OUT_L 0x05
+#define LSM9DS0_TEMP_OUT_H 0x06
+#define LSM9DS0_STATUS_M 0x07
+#define LSM9DS0_OUT_X_L_M 0x08
+#define LSM9DS0_OUT_X_H_M 0x09
+#define LSM9DS0_OUT_Y_L_M 0x0a
+#define LSM9DS0_OUT_Y_H_M 0x0b
+#define LSM9DS0_OUT_Z_L_M 0x0c
+#define LSM9DS0_OUT_Z_H_M 0x0d
+#define LSM9DS0_WHO_AM_I 0x0f
+#define LSM9DS0_INT_CTRL_M 0x12
+#define LSM9DS0_INT_SRC_M 0x13
+#define LSM9DS0_INT_THS_L_M 0x14
+#define LSM9DS0_INT_THS_H_M 0x15
+#define LSM9DS0_OFFSET_X_L_M 0x16
+#define LSM9DS0_OFFSET_X_H_M 0x17
+#define LSM9DS0_OFFSET_Y_L_M 0x18
+#define LSM9DS0_OFFSET_Y_H_M 0x19
+#define LSM9DS0_OFFSET_Z_L_M 0x1a
+#define LSM9DS0_OFFSET_Z_H_M 0x1b
+#define LSM9DS0_REFERENCE_X 0x1c
+#define LSM9DS0_REFERENCE_Y 0x1d
+#define LSM9DS0_REFERENCE_Z 0x1e
+#define LSM9DS0_CTRL0 0x1f
+#define LSM9DS0_CTRL1 0x20
+#define LSM9DS0_CTRL2 0x21
+#define LSM9DS0_CTRL3 0x22
+#define LSM9DS0_CTRL4 0x23
+#define LSM9DS0_CTRL5 0x24
+#define LSM9DS0_CTRL6 0x25
+#define LSM9DS0_CTRL7 0x26
+#define LSM9DS0_STATUS_A 0x27
+#define LSM9DS0_OUT_X_L_A 0x28
+#define LSM9DS0_OUT_X_H_A 0x29
+#define LSM9DS0_OUT_Y_L_A 0x2a
+#define LSM9DS0_OUT_Y_H_A 0x2b
+#define LSM9DS0_OUT_Z_L_A 0x2c
+#define LSM9DS0_OUT_Z_H_A 0x2d
+#define LSM9DS0_FIFO_CTRL 0x2e
+#define LSM9DS0_FIFO_SRC 0x2f
+#define LSM9DS0_IG_CFG1 0x30
+#define LSM9DS0_IG_SRC1 0x31
+#define LSM9DS0_IG_THS1 0x32
+#define LSM9DS0_IG_DUR1 0x33
+#define LSM9DS0_IG_CFG2 0x34
+#define LSM9DS0_IG_SRC2 0x35
+#define LSM9DS0_IG_THS2 0x36
+#define LSM9DS0_IG_DUR2 0x37
+#define LSM9DS0_CLICK_CFG 0x38
+#define LSM9DS0_CLICK_SRC 0x39
+#define LSM9DS0_CLICK_THS 0x3a
+#define LSM9DS0_TIME_LIMIT 0x3b
+#define LSM9DS0_TIME_LATENCY 0x3c
+#define LSM9DS0_TIME_WINDOW 0x3d
+#define LSM9DS0_ACT_THIS 0x3e
+#define LSM9DS0_ACT_DUR 0x3f
+
+// Accel sample rate defines
+
+#define LSM9DS0_ACCEL_SAMPLERATE_3_125 1
+#define LSM9DS0_ACCEL_SAMPLERATE_6_25 2
+#define LSM9DS0_ACCEL_SAMPLERATE_12_5 3
+#define LSM9DS0_ACCEL_SAMPLERATE_25 4
+#define LSM9DS0_ACCEL_SAMPLERATE_50 5
+#define LSM9DS0_ACCEL_SAMPLERATE_100 6
+#define LSM9DS0_ACCEL_SAMPLERATE_200 7
+#define LSM9DS0_ACCEL_SAMPLERATE_400 8
+#define LSM9DS0_ACCEL_SAMPLERATE_800 9
+#define LSM9DS0_ACCEL_SAMPLERATE_1600 10
+
+// Accel FSR
+
+#define LSM9DS0_ACCEL_FSR_2 0
+#define LSM9DS0_ACCEL_FSR_4 1
+#define LSM9DS0_ACCEL_FSR_6 2
+#define LSM9DS0_ACCEL_FSR_8 3
+#define LSM9DS0_ACCEL_FSR_16 4
+
+// Accel filter bandwidth
+
+#define LSM9DS0_ACCEL_LPF_773 0
+#define LSM9DS0_ACCEL_LPF_194 1
+#define LSM9DS0_ACCEL_LPF_362 2
+#define LSM9DS0_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM9DS0_COMPASS_SAMPLERATE_3_125 0
+#define LSM9DS0_COMPASS_SAMPLERATE_6_25 1
+#define LSM9DS0_COMPASS_SAMPLERATE_12_5 2
+#define LSM9DS0_COMPASS_SAMPLERATE_25 3
+#define LSM9DS0_COMPASS_SAMPLERATE_50 4
+#define LSM9DS0_COMPASS_SAMPLERATE_100 5
+
+// Compass FSR
+
+#define LSM9DS0_COMPASS_FSR_2 0
+#define LSM9DS0_COMPASS_FSR_4 1
+#define LSM9DS0_COMPASS_FSR_8 2
+#define LSM9DS0_COMPASS_FSR_12 3
+
+//----------------------------------------------------------
+//
+// LSM9DS1
+
+// I2C Slave Addresses
+
+#define LSM9DS1_ADDRESS0 0x6a
+#define LSM9DS1_ADDRESS1 0x6b
+#define LSM9DS1_ID 0x68
+
+#define LSM9DS1_MAG_ADDRESS0 0x1c
+#define LSM9DS1_MAG_ADDRESS1 0x1d
+#define LSM9DS1_MAG_ADDRESS2 0x1e
+#define LSM9DS1_MAG_ADDRESS3 0x1f
+#define LSM9DS1_MAG_ID 0x3d
+
+// LSM9DS1 Register map
+
+#define LSM9DS1_ACT_THS 0x04
+#define LSM9DS1_ACT_DUR 0x05
+#define LSM9DS1_INT_GEN_CFG_XL 0x06
+#define LSM9DS1_INT_GEN_THS_X_XL 0x07
+#define LSM9DS1_INT_GEN_THS_Y_XL 0x08
+#define LSM9DS1_INT_GEN_THS_Z_XL 0x09
+#define LSM9DS1_INT_GEN_DUR_XL 0x0A
+#define LSM9DS1_REFERENCE_G 0x0B
+#define LSM9DS1_INT1_CTRL 0x0C
+#define LSM9DS1_INT2_CTRL 0x0D
+#define LSM9DS1_WHO_AM_I 0x0F
+#define LSM9DS1_CTRL1 0x10
+#define LSM9DS1_CTRL2 0x11
+#define LSM9DS1_CTRL3 0x12
+#define LSM9DS1_ORIENT_CFG_G 0x13
+#define LSM9DS1_INT_GEN_SRC_G 0x14
+#define LSM9DS1_OUT_TEMP_L 0x15
+#define LSM9DS1_OUT_TEMP_H 0x16
+#define LSM9DS1_STATUS 0x17
+#define LSM9DS1_OUT_X_L_G 0x18
+#define LSM9DS1_OUT_X_H_G 0x19
+#define LSM9DS1_OUT_Y_L_G 0x1A
+#define LSM9DS1_OUT_Y_H_G 0x1B
+#define LSM9DS1_OUT_Z_L_G 0x1C
+#define LSM9DS1_OUT_Z_H_G 0x1D
+#define LSM9DS1_CTRL4 0x1E
+#define LSM9DS1_CTRL5 0x1F
+#define LSM9DS1_CTRL6 0x20
+#define LSM9DS1_CTRL7 0x21
+#define LSM9DS1_CTRL8 0x22
+#define LSM9DS1_CTRL9 0x23
+#define LSM9DS1_CTRL10 0x24
+#define LSM9DS1_INT_GEN_SRC_XL 0x26
+#define LSM9DS1_STATUS2 0x27
+#define LSM9DS1_OUT_X_L_XL 0x28
+#define LSM9DS1_OUT_X_H_XL 0x29
+#define LSM9DS1_OUT_Y_L_XL 0x2A
+#define LSM9DS1_OUT_Y_H_XL 0x2B
+#define LSM9DS1_OUT_Z_L_XL 0x2C
+#define LSM9DS1_OUT_Z_H_XL 0x2D
+#define LSM9DS1_FIFO_CTRL 0x2E
+#define LSM9DS1_FIFO_SRC 0x2F
+#define LSM9DS1_INT_GEN_CFG_G 0x30
+#define LSM9DS1_INT_GEN_THS_XH_G 0x31
+#define LSM9DS1_INT_GEN_THS_XL_G 0x32
+#define LSM9DS1_INT_GEN_THS_YH_G 0x33
+#define LSM9DS1_INT_GEN_THS_YL_G 0x34
+#define LSM9DS1_INT_GEN_THS_ZH_G 0x35
+#define LSM9DS1_INT_GEN_THS_ZL_G 0x36
+#define LSM9DS1_INT_GEN_DUR_G 0x37
+
+// Gyro sample rate defines
+
+#define LSM9DS1_GYRO_SAMPLERATE_14_9 0
+#define LSM9DS1_GYRO_SAMPLERATE_59_5 1
+#define LSM9DS1_GYRO_SAMPLERATE_119 2
+#define LSM9DS1_GYRO_SAMPLERATE_238 3
+#define LSM9DS1_GYRO_SAMPLERATE_476 4
+#define LSM9DS1_GYRO_SAMPLERATE_952 5
+
+// Gyro banwidth defines
+
+#define LSM9DS1_GYRO_BANDWIDTH_0 0
+#define LSM9DS1_GYRO_BANDWIDTH_1 1
+#define LSM9DS1_GYRO_BANDWIDTH_2 2
+#define LSM9DS1_GYRO_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define LSM9DS1_GYRO_FSR_250 0
+#define LSM9DS1_GYRO_FSR_500 1
+#define LSM9DS1_GYRO_FSR_2000 3
+
+// Gyro high pass filter defines
+
+#define LSM9DS1_GYRO_HPF_0 0
+#define LSM9DS1_GYRO_HPF_1 1
+#define LSM9DS1_GYRO_HPF_2 2
+#define LSM9DS1_GYRO_HPF_3 3
+#define LSM9DS1_GYRO_HPF_4 4
+#define LSM9DS1_GYRO_HPF_5 5
+#define LSM9DS1_GYRO_HPF_6 6
+#define LSM9DS1_GYRO_HPF_7 7
+#define LSM9DS1_GYRO_HPF_8 8
+#define LSM9DS1_GYRO_HPF_9 9
+
+// Mag Register Map
+
+#define LSM9DS1_MAG_OFFSET_X_L 0x05
+#define LSM9DS1_MAG_OFFSET_X_H 0x06
+#define LSM9DS1_MAG_OFFSET_Y_L 0x07
+#define LSM9DS1_MAG_OFFSET_Y_H 0x08
+#define LSM9DS1_MAG_OFFSET_Z_L 0x09
+#define LSM9DS1_MAG_OFFSET_Z_H 0x0A
+#define LSM9DS1_MAG_WHO_AM_I 0x0F
+#define LSM9DS1_MAG_CTRL1 0x20
+#define LSM9DS1_MAG_CTRL2 0x21
+#define LSM9DS1_MAG_CTRL3 0x22
+#define LSM9DS1_MAG_CTRL4 0x23
+#define LSM9DS1_MAG_CTRL5 0x24
+#define LSM9DS1_MAG_STATUS 0x27
+#define LSM9DS1_MAG_OUT_X_L 0x28
+#define LSM9DS1_MAG_OUT_X_H 0x29
+#define LSM9DS1_MAG_OUT_Y_L 0x2A
+#define LSM9DS1_MAG_OUT_Y_H 0x2B
+#define LSM9DS1_MAG_OUT_Z_L 0x2C
+#define LSM9DS1_MAG_OUT_Z_H 0x2D
+#define LSM9DS1_MAG_INT_CFG 0x30
+#define LSM9DS1_MAG_INT_SRC 0x31
+#define LSM9DS1_MAG_INT_THS_L 0x32
+#define LSM9DS1_MAG_INT_THS_H 0x33
+
+// Accel sample rate defines
+
+#define LSM9DS1_ACCEL_SAMPLERATE_14_9 1
+#define LSM9DS1_ACCEL_SAMPLERATE_59_5 2
+#define LSM9DS1_ACCEL_SAMPLERATE_119 3
+#define LSM9DS1_ACCEL_SAMPLERATE_238 4
+#define LSM9DS1_ACCEL_SAMPLERATE_476 5
+#define LSM9DS1_ACCEL_SAMPLERATE_952 6
+
+// Accel FSR
+
+#define LSM9DS1_ACCEL_FSR_2 0
+#define LSM9DS1_ACCEL_FSR_16 1
+#define LSM9DS1_ACCEL_FSR_4 2
+#define LSM9DS1_ACCEL_FSR_8 3
+
+// Accel filter bandwidth
+
+#define LSM9DS1_ACCEL_LPF_408 0
+#define LSM9DS1_ACCEL_LPF_211 1
+#define LSM9DS1_ACCEL_LPF_105 2
+#define LSM9DS1_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM9DS1_COMPASS_SAMPLERATE_0_625 0
+#define LSM9DS1_COMPASS_SAMPLERATE_1_25 1
+#define LSM9DS1_COMPASS_SAMPLERATE_2_5 2
+#define LSM9DS1_COMPASS_SAMPLERATE_5 3
+#define LSM9DS1_COMPASS_SAMPLERATE_10 4
+#define LSM9DS1_COMPASS_SAMPLERATE_20 5
+#define LSM9DS1_COMPASS_SAMPLERATE_40 6
+#define LSM9DS1_COMPASS_SAMPLERATE_80 7
+
+// Compass FSR
+
+#define LSM9DS1_COMPASS_FSR_4 0
+#define LSM9DS1_COMPASS_FSR_8 1
+#define LSM9DS1_COMPASS_FSR_12 2
+#define LSM9DS1_COMPASS_FSR_16 3
+
+//----------------------------------------------------------
+//
+// BMX055
+
+// I2C Slave Addresses
+
+#define BMX055_GYRO_ADDRESS0 0x68
+#define BMX055_GYRO_ADDRESS1 0x69
+#define BMX055_GYRO_ID 0x0f
+
+#define BMX055_ACCEL_ADDRESS0 0x18
+#define BMX055_ACCEL_ADDRESS1 0x19
+#define BMX055_ACCEL_ID 0xfa
+
+#define BMX055_MAG_ADDRESS0 0x10
+#define BMX055_MAG_ADDRESS1 0x11
+#define BMX055_MAG_ADDRESS2 0x12
+#define BMX055_MAG_ADDRESS3 0x13
+
+#define BMX055_MAG_ID 0x32
+
+// BMX055 Register map
+
+#define BMX055_GYRO_WHO_AM_I 0x00
+#define BMX055_GYRO_X_LSB 0x02
+#define BMX055_GYRO_X_MSB 0x03
+#define BMX055_GYRO_Y_LSB 0x04
+#define BMX055_GYRO_Y_MSB 0x05
+#define BMX055_GYRO_Z_LSB 0x06
+#define BMX055_GYRO_Z_MSB 0x07
+#define BMX055_GYRO_INT_STATUS_0 0x09
+#define BMX055_GYRO_INT_STATUS_1 0x0a
+#define BMX055_GYRO_INT_STATUS_2 0x0b
+#define BMX055_GYRO_INT_STATUS_3 0x0c
+#define BMX055_GYRO_FIFO_STATUS 0x0e
+#define BMX055_GYRO_RANGE 0x0f
+#define BMX055_GYRO_BW 0x10
+#define BMX055_GYRO_LPM1 0x11
+#define BMX055_GYRO_LPM2 0x12
+#define BMX055_GYRO_RATE_HBW 0x13
+#define BMX055_GYRO_SOFT_RESET 0x14
+#define BMX055_GYRO_INT_EN_0 0x15
+#define BMX055_GYRO_1A 0x1a
+#define BMX055_GYRO_1B 0x1b
+#define BMX055_GYRO_SOC 0x31
+#define BMX055_GYRO_FOC 0x32
+#define BMX055_GYRO_FIFO_CONFIG_0 0x3d
+#define BMX055_GYRO_FIFO_CONFIG_1 0x3e
+#define BMX055_GYRO_FIFO_DATA 0x3f
+
+#define BMX055_ACCEL_WHO_AM_I 0x00
+#define BMX055_ACCEL_X_LSB 0x02
+#define BMX055_ACCEL_X_MSB 0x03
+#define BMX055_ACCEL_Y_LSB 0x04
+#define BMX055_ACCEL_Y_MSB 0x05
+#define BMX055_ACCEL_Z_LSB 0x06
+#define BMX055_ACCEL_Z_MSB 0x07
+#define BMX055_ACCEL_TEMP 0x08
+#define BMX055_ACCEL_INT_STATUS_0 0x09
+#define BMX055_ACCEL_INT_STATUS_1 0x0a
+#define BMX055_ACCEL_INT_STATUS_2 0x0b
+#define BMX055_ACCEL_INT_STATUS_3 0x0c
+#define BMX055_ACCEL_FIFO_STATUS 0x0e
+#define BMX055_ACCEL_PMU_RANGE 0x0f
+#define BMX055_ACCEL_PMU_BW 0x10
+#define BMX055_ACCEL_PMU_LPW 0x11
+#define BMX055_ACCEL_PMU_LOW_POWER 0x12
+#define BMX055_ACCEL_HBW 0x13
+#define BMX055_ACCEL_SOFT_RESET 0x14
+#define BMX055_ACCEL_FIFO_CONFIG_0 0x30
+#define BMX055_ACCEL_OFC_CTRL 0x36
+#define BMX055_ACCEL_OFC_SETTING 0x37
+#define BMX055_ACCEL_FIFO_CONFIG_1 0x3e
+#define BMX055_ACCEL_FIFO_DATA 0x3f
+
+#define BMX055_MAG_WHO_AM_I 0x40
+#define BMX055_MAG_X_LSB 0x42
+#define BMX055_MAG_X_MSB 0x43
+#define BMX055_MAG_Y_LSB 0x44
+#define BMX055_MAG_Y_MSB 0x45
+#define BMX055_MAG_Z_LSB 0x46
+#define BMX055_MAG_Z_MSB 0x47
+#define BMX055_MAG_RHALL_LSB 0x48
+#define BMX055_MAG_RHALL_MSB 0x49
+#define BMX055_MAG_INT_STAT 0x4a
+#define BMX055_MAG_POWER 0x4b
+#define BMX055_MAG_MODE 0x4c
+#define BMX055_MAG_INT_ENABLE 0x4d
+#define BMX055_MAG_AXIS_ENABLE 0x4e
+#define BMX055_MAG_REPXY 0x51
+#define BMX055_MAG_REPZ 0x52
+
+#define BMX055_MAG_DIG_X1 0x5D
+#define BMX055_MAG_DIG_Y1 0x5E
+#define BMX055_MAG_DIG_Z4_LSB 0x62
+#define BMX055_MAG_DIG_Z4_MSB 0x63
+#define BMX055_MAG_DIG_X2 0x64
+#define BMX055_MAG_DIG_Y2 0x65
+#define BMX055_MAG_DIG_Z2_LSB 0x68
+#define BMX055_MAG_DIG_Z2_MSB 0x69
+#define BMX055_MAG_DIG_Z1_LSB 0x6A
+#define BMX055_MAG_DIG_Z1_MSB 0x6B
+#define BMX055_MAG_DIG_XYZ1_LSB 0x6C
+#define BMX055_MAG_DIG_XYZ1_MSB 0x6D
+#define BMX055_MAG_DIG_Z3_LSB 0x6E
+#define BMX055_MAG_DIG_Z3_MSB 0x6F
+#define BMX055_MAG_DIG_XY2 0x70
+#define BMX055_MAG_DIG_XY1 0x71
+
+// Gyro sample rate defines
+
+#define BMX055_GYRO_SAMPLERATE_100_32 0x07
+#define BMX055_GYRO_SAMPLERATE_200_64 0x06
+#define BMX055_GYRO_SAMPLERATE_100_12 0x05
+#define BMX055_GYRO_SAMPLERATE_200_23 0x04
+#define BMX055_GYRO_SAMPLERATE_400_47 0x03
+#define BMX055_GYRO_SAMPLERATE_1000_116 0x02
+#define BMX055_GYRO_SAMPLERATE_2000_230 0x01
+#define BMX055_GYRO_SAMPLERATE_2000_523 0x00
+
+// Gyro FSR defines
+
+#define BMX055_GYRO_FSR_2000 0x00
+#define BMX055_GYRO_FSR_1000 0x01
+#define BMX055_GYRO_FSR_500 0x02
+#define BMX055_GYRO_FSR_250 0x03
+#define BMX055_GYRO_FSR_125 0x04
+
+// Accel sample rate defines
+
+#define BMX055_ACCEL_SAMPLERATE_15 0X00
+#define BMX055_ACCEL_SAMPLERATE_31 0X01
+#define BMX055_ACCEL_SAMPLERATE_62 0X02
+#define BMX055_ACCEL_SAMPLERATE_125 0X03
+#define BMX055_ACCEL_SAMPLERATE_250 0X04
+#define BMX055_ACCEL_SAMPLERATE_500 0X05
+#define BMX055_ACCEL_SAMPLERATE_1000 0X06
+#define BMX055_ACCEL_SAMPLERATE_2000 0X07
+
+// Accel FSR defines
+
+#define BMX055_ACCEL_FSR_2 0x00
+#define BMX055_ACCEL_FSR_4 0x01
+#define BMX055_ACCEL_FSR_8 0x02
+#define BMX055_ACCEL_FSR_16 0x03
+
+
+// Mag preset defines
+
+#define BMX055_MAG_LOW_POWER 0x00
+#define BMX055_MAG_REGULAR 0x01
+#define BMX055_MAG_ENHANCED 0x02
+#define BMX055_MAG_HIGH_ACCURACY 0x03
+
+//----------------------------------------------------------
+//
+// BNO055
+
+// I2C Slave Addresses
+
+#define BNO055_ADDRESS0 0x28
+#define BNO055_ADDRESS1 0x29
+#define BNO055_ID 0xa0
+
+// Register map
+
+#define BNO055_WHO_AM_I 0x00
+#define BNO055_PAGE_ID 0x07
+#define BNO055_ACCEL_DATA 0x08
+#define BNO055_MAG_DATA 0x0e
+#define BNO055_GYRO_DATA 0x14
+#define BNO055_FUSED_EULER 0x1a
+#define BNO055_FUSED_QUAT 0x20
+#define BNO055_UNIT_SEL 0x3b
+#define BNO055_OPER_MODE 0x3d
+#define BNO055_PWR_MODE 0x3e
+#define BNO055_SYS_TRIGGER 0x3f
+#define BNO055_AXIS_MAP_CONFIG 0x41
+#define BNO055_AXIS_MAP_SIGN 0x42
+
+// Operation modes
+
+#define BNO055_OPER_MODE_CONFIG 0x00
+#define BNO055_OPER_MODE_NDOF 0x0c
+
+// Power modes
+
+#define BNO055_PWR_MODE_NORMAL 0x00
+
+#endif // _RTIMUDEFS_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
new file mode 100644
index 0000000..b8e32e6
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
@@ -0,0 +1,563 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMUGD20HM303D.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUGD20HM303D::RTIMUGD20HM303D(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMUGD20HM303D::~RTIMUGD20HM303D()
+{
+}
+
+bool RTIMUGD20HM303D::IMUInit()
+{
+ unsigned char result;
+
+#ifdef GD20HM303D_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+
+ // work out accel/mag address
+
+ if (m_settings->HALRead(LSM303D_ADDRESS0, LSM303D_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM303D_ID) {
+ m_accelCompassSlaveAddr = LSM303D_ADDRESS0;
+ }
+ } else {
+ m_accelCompassSlaveAddr = LSM303D_ADDRESS1;
+ }
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
+ return false;
+
+ if (result != L3GD20H_ID) {
+ HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel/compass
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result, "Failed to read LSM303D id"))
+ return false;
+
+ if (result != LSM303D_ID) {
+ HAL_ERROR1("Incorrect LSM303D id %d\n", result);
+ return false;
+ }
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL2())
+ return false;
+
+ if (!setCompassCTRL5())
+ return false;
+
+ if (!setCompassCTRL6())
+ return false;
+
+ if (!setCompassCTRL7())
+ return false;
+
+#ifdef GD20HM303D_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20H FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("GD20HM303D init complete\n");
+ return true;
+}
+
+bool RTIMUGD20HM303D::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+ unsigned char lowOdr = 0;
+
+ switch (m_settings->m_GD20HM303DGyroSampleRate) {
+ case L3GD20H_SAMPLERATE_12_5:
+ ctrl1 = 0x0f;
+ lowOdr = 1;
+ m_sampleRate = 13;
+ break;
+
+ case L3GD20H_SAMPLERATE_25:
+ ctrl1 = 0x4f;
+ lowOdr = 1;
+ m_sampleRate = 25;
+ break;
+
+ case L3GD20H_SAMPLERATE_50:
+ ctrl1 = 0x8f;
+ lowOdr = 1;
+ m_sampleRate = 50;
+ break;
+
+ case L3GD20H_SAMPLERATE_100:
+ ctrl1 = 0x0f;
+ m_sampleRate = 100;
+ break;
+
+ case L3GD20H_SAMPLERATE_200:
+ ctrl1 = 0x4f;
+ m_sampleRate = 200;
+ break;
+
+ case L3GD20H_SAMPLERATE_400:
+ ctrl1 = 0x8f;
+ m_sampleRate = 400;
+ break;
+
+ case L3GD20H_SAMPLERATE_800:
+ ctrl1 = 0xcf;
+ m_sampleRate = 800;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H sample rate code %d\n", m_settings->m_GD20HM303DGyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_GD20HM303DGyroBW) {
+ case L3GD20H_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case L3GD20H_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case L3GD20H_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case L3GD20H_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr, "Failed to set L3GD20H LOW_ODR"))
+ return false;
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1, "Failed to set L3GD20H CTRL1"));
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL2()
+{
+ if ((m_settings->m_GD20HM303DGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DGyroHpf > L3GD20H_HPF_9)) {
+ HAL_ERROR1("Illegal L3GD20H high pass filter code %d\n", m_settings->m_GD20HM303DGyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL2, m_settings->m_GD20HM303DGyroHpf, "Failed to set L3GD20H CTRL2");
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DGyroFsr) {
+ case L3GD20H_FSR_245:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H FSR code %d\n", m_settings->m_GD20HM303DGyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL4, ctrl4, "Failed to set L3GD20H CTRL4");
+}
+
+
+bool RTIMUGD20HM303D::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef GD20HM303D_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, ctrl5, "Failed to set L3GD20H CTRL5");
+}
+
+
+bool RTIMUGD20HM303D::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_GD20HM303DAccelSampleRate < 0) || (m_settings->m_GD20HM303DAccelSampleRate > 10)) {
+ HAL_ERROR1("Illegal LSM303D accel sample rate code %d\n", m_settings->m_GD20HM303DAccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_GD20HM303DAccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL1, ctrl1, "Failed to set LSM303D CTRL1");
+}
+
+bool RTIMUGD20HM303D::setAccelCTRL2()
+{
+ unsigned char ctrl2;
+
+ if ((m_settings->m_GD20HM303DAccelLpf < 0) || (m_settings->m_GD20HM303DAccelLpf > 3)) {
+ HAL_ERROR1("Illegal LSM303D accel low pass fiter code %d\n", m_settings->m_GD20HM303DAccelLpf);
+ return false;
+ }
+
+ switch (m_settings->m_GD20HM303DAccelFsr) {
+ case LSM303D_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.000061;
+ break;
+
+ case LSM303D_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.000122;
+ break;
+
+ case LSM303D_ACCEL_FSR_6:
+ m_accelScale = (RTFLOAT)0.000183;
+ break;
+
+ case LSM303D_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.000244;
+ break;
+
+ case LSM303D_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.000732;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303D accel FSR code %d\n", m_settings->m_GD20HM303DAccelFsr);
+ return false;
+ }
+
+ ctrl2 = (m_settings->m_GD20HM303DAccelLpf << 6) | (m_settings->m_GD20HM303DAccelFsr << 3);
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL2, ctrl2, "Failed to set LSM303D CTRL2");
+}
+
+
+bool RTIMUGD20HM303D::setCompassCTRL5()
+{
+ unsigned char ctrl5;
+
+ if ((m_settings->m_GD20HM303DCompassSampleRate < 0) || (m_settings->m_GD20HM303DCompassSampleRate > 5)) {
+ HAL_ERROR1("Illegal LSM303D compass sample rate code %d\n", m_settings->m_GD20HM303DCompassSampleRate);
+ return false;
+ }
+
+ ctrl5 = (m_settings->m_GD20HM303DCompassSampleRate << 2);
+
+#ifdef GD20HM303D_CACHE_MODE
+ // enable fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL5, ctrl5, "Failed to set LSM303D CTRL5");
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL6()
+{
+ unsigned char ctrl6;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_GD20HM303DCompassFsr) {
+ case LSM303D_COMPASS_FSR_2:
+ ctrl6 = 0;
+ m_compassScale = (RTFLOAT)0.008;
+ break;
+
+ case LSM303D_COMPASS_FSR_4:
+ ctrl6 = 0x20;
+ m_compassScale = (RTFLOAT)0.016;
+ break;
+
+ case LSM303D_COMPASS_FSR_8:
+ ctrl6 = 0x40;
+ m_compassScale = (RTFLOAT)0.032;
+ break;
+
+ case LSM303D_COMPASS_FSR_12:
+ ctrl6 = 0x60;
+ m_compassScale = (RTFLOAT)0.0479;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303D compass FSR code %d\n", m_settings->m_GD20HM303DCompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL6, ctrl6, "Failed to set LSM303D CTRL6");
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL7()
+{
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL7, 0x60, "Failed to set LSM303D CTRL7");
+}
+
+
+int RTIMUGD20HM303D::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303D::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef GD20HM303D_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_FIFO_SRC, 1, &status, "Failed to read L3GD20H fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("L3GD20H fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x10, "Failed to set L3GD20H CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x0, "Failed to set L3GD20H FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20H FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < GD20HM303D_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6, accelData, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6, compassData, "Failed to read LSM303D compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= GD20HM303D_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == GD20HM303D_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, GD20HM303D_FIFO_CHUNK_SIZE * GD20HM303D_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read L3GD20H fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM303D compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = GD20HM303D_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20HM303D_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += GD20HM303D_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status, "Failed to read L3GD20H status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6, accelData, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6, compassData, "Failed to read LSM303D compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+ RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ m_imuData.compass.setY(-m_imuData.compass.y());
+ m_imuData.compass.setZ(-m_imuData.compass.z());
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
new file mode 100644
index 0000000..7c182f5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
@@ -0,0 +1,95 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUGD20HM303D_H
+#define _RTIMUGD20HM303D_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define GD20HM303D_CACHE_MODE // not reliable at the moment
+
+#ifdef GD20HM303D_CACHE_MODE
+
+// Cache defs
+
+#define GD20HM303D_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define GD20HM303D_FIFO_THRESH 16 // threshold point in fifo
+#define GD20HM303D_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[GD20HM303D_FIFO_THRESH * GD20HM303D_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} GD20HM303D_CACHE_BLOCK;
+
+#endif
+
+class RTIMUGD20HM303D : public RTIMU
+{
+public:
+ RTIMUGD20HM303D(RTIMUSettings *settings);
+ ~RTIMUGD20HM303D();
+
+ virtual const char *IMUName() { return "L3GD20H + LSM303D"; }
+ virtual int IMUType() { return RTIMU_TYPE_GD20HM303D; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL2();
+ bool setCompassCTRL5();
+ bool setCompassCTRL6();
+ bool setCompassCTRL7();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of L3GD20H
+ unsigned char m_accelCompassSlaveAddr; // I2C address of LSM303D
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScale;
+
+#ifdef GD20HM303D_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ GD20HM303D_CACHE_BLOCK m_cache[GD20HM303D_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMUGD20HM303D_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
new file mode 100644
index 0000000..aeb4be6
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
@@ -0,0 +1,562 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMUGD20HM303DLHC.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUGD20HM303DLHC::RTIMUGD20HM303DLHC(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMUGD20HM303DLHC::~RTIMUGD20HM303DLHC()
+{
+}
+
+bool RTIMUGD20HM303DLHC::IMUInit()
+{
+ unsigned char result;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+ m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
+ m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
+ return false;
+
+ if (result != L3GD20H_ID) {
+ HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL4())
+ return false;
+
+ // Set up the compass
+
+ if (!setCompassCRA())
+ return false;
+
+ if (!setCompassCRB())
+ return false;
+
+ if (!setCompassCRM())
+ return false;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("GD20HM303DLHC init complete\n");
+ return true;
+}
+
+bool RTIMUGD20HM303DLHC::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+ unsigned char lowOdr = 0;
+
+ switch (m_settings->m_GD20HM303DLHCGyroSampleRate) {
+ case L3GD20H_SAMPLERATE_12_5:
+ ctrl1 = 0x0f;
+ lowOdr = 1;
+ m_sampleRate = 13;
+ break;
+
+ case L3GD20H_SAMPLERATE_25:
+ ctrl1 = 0x4f;
+ lowOdr = 1;
+ m_sampleRate = 25;
+ break;
+
+ case L3GD20H_SAMPLERATE_50:
+ ctrl1 = 0x8f;
+ lowOdr = 1;
+ m_sampleRate = 50;
+ break;
+
+ case L3GD20H_SAMPLERATE_100:
+ ctrl1 = 0x0f;
+ m_sampleRate = 100;
+ break;
+
+ case L3GD20H_SAMPLERATE_200:
+ ctrl1 = 0x4f;
+ m_sampleRate = 200;
+ break;
+
+ case L3GD20H_SAMPLERATE_400:
+ ctrl1 = 0x8f;
+ m_sampleRate = 400;
+ break;
+
+ case L3GD20H_SAMPLERATE_800:
+ ctrl1 = 0xcf;
+ m_sampleRate = 800;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H sample rate code %d\n", m_settings->m_GD20HM303DLHCGyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_GD20HM303DLHCGyroBW) {
+ case L3GD20H_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case L3GD20H_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case L3GD20H_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case L3GD20H_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr, "Failed to set L3GD20H LOW_ODR"))
+ return false;
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1, "Failed to set L3GD20H CTRL1"));
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL2()
+{
+ if ((m_settings->m_GD20HM303DLHCGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DLHCGyroHpf > L3GD20H_HPF_9)) {
+ HAL_ERROR1("Illegal L3GD20H high pass filter code %d\n", m_settings->m_GD20HM303DLHCGyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL2, m_settings->m_GD20HM303DLHCGyroHpf, "Failed to set L3GD20H CTRL2");
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DLHCGyroFsr) {
+ case L3GD20H_FSR_245:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H FSR code %d\n", m_settings->m_GD20HM303DLHCGyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL4, ctrl4, "Failed to set L3GD20H CTRL4");
+}
+
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, ctrl5, "Failed to set L3GD20H CTRL5");
+}
+
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_GD20HM303DLHCAccelSampleRate < 1) || (m_settings->m_GD20HM303DLHCAccelSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC accel sample rate code %d\n", m_settings->m_GD20HM303DLHCAccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_GD20HM303DLHCAccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL1_A, ctrl1, "Failed to set LSM303D CTRL1");
+}
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DLHCAccelFsr) {
+ case LSM303DLHC_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)16;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC accel FSR code %d\n", m_settings->m_GD20HM303DLHCAccelFsr);
+ return false;
+ }
+
+ ctrl4 = 0x80 + (m_settings->m_GD20HM303DLHCAccelFsr << 4);
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4");
+}
+
+
+bool RTIMUGD20HM303DLHC::setCompassCRA()
+{
+ unsigned char cra;
+
+ if ((m_settings->m_GD20HM303DLHCCompassSampleRate < 0) || (m_settings->m_GD20HM303DLHCCompassSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC compass sample rate code %d\n", m_settings->m_GD20HM303DLHCCompassSampleRate);
+ return false;
+ }
+
+ cra = (m_settings->m_GD20HM303DLHCCompassSampleRate << 2);
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRA_M, cra, "Failed to set LSM303DLHC CRA_M");
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRB()
+{
+ unsigned char crb;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_GD20HM303DLHCCompassFsr) {
+ case LSM303DLHC_COMPASS_FSR_1_3:
+ crb = 0x20;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_1_9:
+ crb = 0x40;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_2_5:
+ crb = 0x60;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4:
+ crb = 0x80;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4_7:
+ crb = 0xa0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_5_6:
+ crb = 0xc0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_8_1:
+ crb = 0xe0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC compass FSR code %d\n", m_settings->m_GD20HM303DLHCCompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRB_M, crb, "Failed to set LSM303DLHC CRB_M");
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRM()
+{
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRM_M, 0x00, "Failed to set LSM303DLHC CRM_M");
+}
+
+
+int RTIMUGD20HM303DLHC::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303DLHC::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_FIFO_SRC, 1, &status, "Failed to read L3GD20 fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("L3GD20 fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x10, "Failed to set L3GD20 CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < GD20HM303DLHC_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= GD20HM303DLHC_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == GD20HM303DLHC_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, GD20HM303DLHC_FIFO_CHUNK_SIZE * GD20HM303DLHC_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read L3GD20 fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = GD20HM303DLHC_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20HM303DLHC_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += GD20HM303DLHC_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status, "Failed to read L3GD20H status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+
+ m_imuData.compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
+ m_imuData.compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
+ m_imuData.compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ RTFLOAT temp;
+
+ temp = m_imuData.compass.z();
+ m_imuData.compass.setZ(-m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h
new file mode 100644
index 0000000..88876c5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.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 _RTIMUGD20HM303DLHC_H
+#define _RTIMUGD20HM303DLHC_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define GD20M303DLHC_CACHE_MODE // not reliable at the moment
+
+
+#ifdef GD20M303DLHC_CACHE_MODE
+
+// Cache defs
+
+#define GD20M303DLHC_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define GD20M303DLHC_FIFO_THRESH 16 // threshold point in fifo
+#define GD20M303DLHC_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[GD20M303DLHC_FIFO_THRESH * GD20M303DLHC_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} GD20M303DLHC_CACHE_BLOCK;
+
+#endif
+
+class RTIMUGD20HM303DLHC : public RTIMU
+{
+public:
+ RTIMUGD20HM303DLHC(RTIMUSettings *settings);
+ ~RTIMUGD20HM303DLHC();
+
+ virtual const char *IMUName() { return "L3GD20H + LSM303DLHC"; }
+ virtual int IMUType() { return RTIMU_TYPE_GD20HM303DLHC; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL4();
+ bool setCompassCRA();
+ bool setCompassCRB();
+ bool setCompassCRM();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of L3GD20
+ unsigned char m_accelSlaveAddr; // I2C address of LSM303DLHC accel
+ unsigned char m_compassSlaveAddr; // I2C address of LSM303DLHC compass
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScaleXY;
+ RTFLOAT m_compassScaleZ;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ GD20M303DLHC_CACHE_BLOCK m_cache[GD20M303DLHC_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMUGD20HM303DLHC_H