c2-utopia/lib/RTIMULib/RTFusionRTQF.h

102 lines
4.5 KiB
C++

////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// 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.
#ifndef _RTFUSIONRTQF_H
#define _RTFUSIONRTQF_H
#ifndef RTARDULINK_MODE
#include "RTMath.h"
// Define this symbol to use more scientific prediction correction
#define USE_SLERP
class RTFusionRTQF
{
public:
RTFusionRTQF();
~RTFusionRTQF();
// 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(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp);
// 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;}
#ifdef USE_SLERP
// the following function can be called to set the SLERP power
void setSlerpPower(RTFLOAT power) { m_slerpPower = power; }
#else
// the following two functions can be called to customize the noise covariance
void setQ(RTFLOAT Q) { m_Q = Q; reset();}
void setR(RTFLOAT R) { if (R > 0) m_R = R; reset();}
#endif
inline const RTVector3& getMeasuredPose() {return m_measuredPose;}
inline const RTQuaternion& getMeasuredQPose() {return m_measuredQPose;}
inline const RTVector3& getFusionPose() {return m_fusionPose;}
inline const RTQuaternion& getFusionQPose() {return m_fusionQPose;}
private:
void calculatePose(const RTVector3& accel, const RTVector3& mag); // generates pose from accels and heading
RTFLOAT m_timeDelta; // time between predictions
RTQuaternion m_stateQError; // difference between stateQ and measuredQ
#ifdef USE_SLERP
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
#else
RTFLOAT m_Q; // process noise covariance
RTFLOAT m_R; // the measurement noise covariance
#endif
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
bool m_enableGyro; // enables gyro as input
bool m_enableAccel; // enables accel as input
bool m_enableCompass; // enables compass a input
bool m_firstTime; // if first time after reset
unsigned long m_lastFusionTime; // for delta time calculation
};
#endif // #ifndef RTARDULINK_MODE
#endif // _RTFUSIONRTQF_H