You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
126 lines
6.4 KiB
126 lines
6.4 KiB
#ifndef USFS_h |
|
#define USFSh_h |
|
|
|
#include "Arduino.h" |
|
#include "Wire.h" |
|
#include "I2Cdev.h" |
|
|
|
// EM7180 SENtral register map |
|
// see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf |
|
// |
|
#define EM7180_QX 0x00 // this is a 32-bit normalized floating point number read from registers 0x00-03 |
|
#define EM7180_QY 0x04 // this is a 32-bit normalized floating point number read from registers 0x04-07 |
|
#define EM7180_QZ 0x08 // this is a 32-bit normalized floating point number read from registers 0x08-0B |
|
#define EM7180_QW 0x0C // this is a 32-bit normalized floating point number read from registers 0x0C-0F |
|
#define EM7180_QTIME 0x10 // this is a 16-bit unsigned integer read from registers 0x10-11 |
|
#define EM7180_MX 0x12 // int16_t from registers 0x12-13 |
|
#define EM7180_MY 0x14 // int16_t from registers 0x14-15 |
|
#define EM7180_MZ 0x16 // int16_t from registers 0x16-17 |
|
#define EM7180_MTIME 0x18 // uint16_t from registers 0x18-19 |
|
#define EM7180_AX 0x1A // int16_t from registers 0x1A-1B |
|
#define EM7180_AY 0x1C // int16_t from registers 0x1C-1D |
|
#define EM7180_AZ 0x1E // int16_t from registers 0x1E-1F |
|
#define EM7180_ATIME 0x20 // uint16_t from registers 0x20-21 |
|
#define EM7180_GX 0x22 // int16_t from registers 0x22-23 |
|
#define EM7180_GY 0x24 // int16_t from registers 0x24-25 |
|
#define EM7180_GZ 0x26 // int16_t from registers 0x26-27 |
|
#define EM7180_GTIME 0x28 // uint16_t from registers 0x28-29 |
|
#define EM7180_Baro 0x2A // start of two-byte MS5637 pressure data, 16-bit signed interger |
|
#define EM7180_BaroTIME 0x2C // start of two-byte MS5637 pressure timestamp, 16-bit unsigned |
|
#define EM7180_Temp 0x2E // start of two-byte MS5637 temperature data, 16-bit signed interger |
|
#define EM7180_TempTIME 0x30 // start of two-byte MS5637 temperature timestamp, 16-bit unsigned |
|
#define EM7180_QRateDivisor 0x32 // uint8_t |
|
#define EM7180_EnableEvents 0x33 |
|
#define EM7180_HostControl 0x34 |
|
#define EM7180_EventStatus 0x35 |
|
#define EM7180_SensorStatus 0x36 |
|
#define EM7180_SentralStatus 0x37 |
|
#define EM7180_AlgorithmStatus 0x38 |
|
#define EM7180_FeatureFlags 0x39 |
|
#define EM7180_ParamAcknowledge 0x3A |
|
#define EM7180_SavedParamByte0 0x3B |
|
#define EM7180_SavedParamByte1 0x3C |
|
#define EM7180_SavedParamByte2 0x3D |
|
#define EM7180_SavedParamByte3 0x3E |
|
#define EM7180_ActualMagRate 0x45 |
|
#define EM7180_ActualAccelRate 0x46 |
|
#define EM7180_ActualGyroRate 0x47 |
|
#define EM7180_ActualBaroRate 0x48 |
|
#define EM7180_ActualTempRate 0x49 |
|
#define EM7180_ErrorRegister 0x50 |
|
#define EM7180_AlgorithmControl 0x54 |
|
#define EM7180_MagRate 0x55 |
|
#define EM7180_AccelRate 0x56 |
|
#define EM7180_GyroRate 0x57 |
|
#define EM7180_BaroRate 0x58 |
|
#define EM7180_TempRate 0x59 |
|
#define EM7180_LoadParamByte0 0x60 |
|
#define EM7180_LoadParamByte1 0x61 |
|
#define EM7180_LoadParamByte2 0x62 |
|
#define EM7180_LoadParamByte3 0x63 |
|
#define EM7180_ParamRequest 0x64 |
|
#define EM7180_ROMVersion1 0x70 |
|
#define EM7180_ROMVersion2 0x71 |
|
#define EM7180_RAMVersion1 0x72 |
|
#define EM7180_RAMVersion2 0x73 |
|
#define EM7180_ProductID 0x90 |
|
#define EM7180_RevisionID 0x91 |
|
#define EM7180_RunStatus 0x92 |
|
#define EM7180_UploadAddress 0x94 // uint16_t registers 0x94 (MSB)-5(LSB) |
|
#define EM7180_UploadData 0x96 |
|
#define EM7180_CRCHost 0x97 // uint32_t from registers 0x97-9A |
|
#define EM7180_ResetRequest 0x9B |
|
#define EM7180_PassThruStatus 0x9E |
|
#define EM7180_PassThruControl 0xA0 |
|
#define EM7180_ACC_LPF_BW 0x5B //Register GP36 |
|
#define EM7180_GYRO_LPF_BW 0x5C //Register GP37 |
|
#define EM7180_BARO_LPF_BW 0x5D //Register GP38 |
|
|
|
#define EM7180_ADDRESS 0x28 // Address of the EM7180 SENtral sensor hub |
|
#define M24512DFM_DATA_ADDRESS 0x50 // Address of the 500 page M24512DFM EEPROM data buffer, 1024 bits (128 8-bit bytes) per page |
|
#define M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DFM lockable EEPROM ID page |
|
#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 |
|
#define AK8963_ADDRESS 0x0C // Address of magnetometer |
|
#define MS5637_ADDRESS 0x76 // Address of altimeter |
|
|
|
class USFS |
|
{ |
|
public: |
|
USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus); |
|
float uint32_reg_to_float (uint8_t *buf); |
|
float int32_reg_to_float (uint8_t *buf); |
|
void float_to_bytes (float param_val, uint8_t *buf); |
|
void EM7180_set_gyro_FS (uint16_t gyro_fs); |
|
void EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs); |
|
void EM7180_set_integer_param (uint8_t param, uint32_t param_val); |
|
void EM7180_set_float_param (uint8_t param, float param_val); |
|
void readSENtralQuatData(float * destination); |
|
void readSENtralAccelData(int16_t * destination); |
|
void readSENtralGyroData(int16_t * destination); |
|
void readSENtralMagData(int16_t * destination); |
|
void initEM7180(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt, uint8_t gyroRt, uint8_t baroRt); |
|
int16_t readSENtralBaroData(); |
|
int16_t readSENtralTempData(); |
|
void SENtralPassThroughMode(); |
|
void M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t data); |
|
void M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest); |
|
uint8_t M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2); |
|
void M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest); |
|
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
|
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
|
void getChipID(); |
|
void loadfwfromEEPROM(); |
|
uint8_t checkEM7180Status(); |
|
uint8_t checkEM7180Errors(); |
|
private: |
|
uint8_t _intPin; |
|
bool _passThru; |
|
float _q[4]; |
|
float _beta; |
|
float _deltat; |
|
float _Kp; |
|
float _Ki; |
|
I2Cdev* _i2c_bus; |
|
}; |
|
|
|
#endif
|
|
|