-
Notifications
You must be signed in to change notification settings - Fork 0
/
imu.h
154 lines (133 loc) · 4.74 KB
/
imu.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#ifndef IMU_H_
#define IMU_H_
#define Device_Address 0x68 /*Device Address/Identifier for MPU9250*/
#define PWR_MGMT_1 0x6B
#define PWR_MGMT_2 0x6C
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define INT_ENABLE 0x38
#define ACCEL_XOUT_H 0x3B
#define ACCEL_YOUT_H 0x3D
#define ACCEL_ZOUT_H 0x3F
#define GYRO_XOUT_H 0x43
#define GYRO_YOUT_H 0x45
#define GYRO_ZOUT_H 0x47
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define FIFO_COUNTH 0x72
#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E
#define INT_PIN_CFG 0x37
#define ACCEL_CONFIG2 0x1D
//Magnetometer Registers
#define AK8963_ADDRESS 0x0C
#define AK8963_WHO_AM_I 0x00 // should return 0x48
#define AK8963_INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
#define INT_STATUS 0x3A
#define I2C_DEVICE 1
#define I2C_FLAGS 0
#include <math.h>
#include <time.h>
#include <queue>
#include "kalmanFilter.h"
using namespace std;
//Class IMU
class IMU {
private:
int identifier, mag_identifier;
void initMagnetometer();
void calibrateMagnetometer();
void calibrateMagnetometer_SET(int IMU_NUM);
public:
IMU();
struct Data {
float X, Y, Z;
};
struct EuAngles {
float Roll, Pitch, Yaw;
};
struct KalmanData {
Kalman Roll, Pitch, Yaw; //Roll - rot around the x-axis //Pitch - rot around y-axis
};
queue<float> AccX, AccY, AccZ;
struct timespec now;
float MagCorrectionVal[3][3];
float deltat;
void setIdentifiers(int ID, int MAG_ID);
void getIdentifier(int *ID);
void getMagIdentifier(int *MAG_ID);
void calibrateMPU9250();
void readRawIMU();
void applySensitivityFactor();
void initCalibMagnetometer(int IMU_NUM);
void lowPassFilter();
// void setOffsetQuaternions();
Data AccelData; //Accelerometer Data
Data InitialGVector; //Initial GVector
Data AccelBias; //Accelerometer Bias
Data GyroData; //Gyroscope Data
Data Angles; //Angle Computations
Data MagData; //Magnetometer Data
Data MagCalib; //Magnetometer Calibration
Data MagBias, MagScale; //Magnetometer Bias and Scale
KalmanData Kalman; //Kalman Data
EuAngles EulerAngles; //Initial Euler Angles
};
void pushNewData(queue<float> *q, float newData);
void populateQueue(queue<float> *q, int size);
//Function Prototypes
void MPU9250_Init(int ID);
void calculateUsingAccAngles(IMU *IMU);
short read_raw_data(int IMUIdentifier, int addr);
// Functions for IMU
void setupMultiplexer();
void selectIMU(unsigned char byte);
void getIMUData(IMU *IMU);
// float getElapsedTime(struct timespec *prev);
void readIMU(IMU *IMU, int IMU_NUM);
void initDevice(IMU *IMU, int IMU_NUM);
// Operation Functions
void normalizeVectors(float rV[3]);
float computeAngle(IMU* IMU);
float vectorDotProd(float v1[3], float v2[3]);
void vectorCross(float v[2][3], float rV[3]);
void readIMU(IMU *IMU, int IMU_NUM);
void initKalmanValues(IMU *IMU, int IMU_NUM);
void rotMatrixRoll(IMU *IMU);
void rotMatrixYaw(IMU *IMU);
void rotMatrixPitch(IMU *IMU);
void applyRotMatrix(IMU *IMU);
float computeAngle(IMU* IMU);
float conv(queue<float> Acc, float* h);
// Filter
void computeEulerAngles(IMU *IMU);
void applyKalmanFiltering(IMU *IMU);
void computeYaw(IMU *IMU);
#endif