201 lines
9.0 KiB
C++
201 lines
9.0 KiB
C++
////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// 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.
|
|
|
|
#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
|