350 lines
16 KiB
C++
350 lines
16 KiB
C++
////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// This file is part of RTIMULib-Teensy
|
|
//
|
|
// 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 MPU-9250 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"
|
|
#include <SD.h>
|
|
#include <SPI.h>
|
|
#include <EEPROM.h>
|
|
|
|
#define SD_CHIP_SELECT 10
|
|
#define IMU_CHIP_SELECT 9
|
|
|
|
// Defines for EEPROM config
|
|
|
|
#define RTIMULIB_CAL_DATA_VALID_LOW 0xfc // pattern to detect valid config - low byte
|
|
#define RTIMULIB_CAL_DATA_VALID_HIGH 0x15 // pattern to detect valid config - high byte
|
|
|
|
typedef struct
|
|
{
|
|
unsigned char validL; // should contain the valid pattern if a good config
|
|
unsigned char validH; // should contain the valid pattern if a good config
|
|
unsigned char magValid; // true if data valid
|
|
unsigned char pad;
|
|
RTFLOAT magMin[3]; // min values
|
|
RTFLOAT magMax[3]; // max values
|
|
} RTIMULIB_CAL_DATA;
|
|
|
|
// Settings keys for SD card based config
|
|
|
|
#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"
|
|
|
|
// 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"
|
|
|
|
// 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 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:
|
|
RTIMUSettings(const char *productType = "RTIMULib");
|
|
|
|
// 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 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
|
|
|
|
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
|
|
|
|
// 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;
|
|
bool m_usingSD; // true if using SD card
|
|
|
|
void EEErase(byte device);
|
|
void EEWrite(byte device, RTIMULIB_CAL_DATA * calData);
|
|
boolean EERead(byte device, RTIMULIB_CAL_DATA * calData);
|
|
};
|
|
|
|
#endif // _RTIMUSETTINGS_H
|
|
|