Simple example to show how to get an estimation of the attitude with a 9DOF IMU and the Kalman filter
Dependencies: L3GD20 LSM303DLHC mbed-dsp mbed
Fork of minimu-9v2 by
main.cpp@1:ba2d31e3112d, 2017-03-25 (annotated)
- Committer:
- capriele
- Date:
- Sat Mar 25 16:48:32 2017 +0000
- Revision:
- 1:ba2d31e3112d
- Parent:
- 0:4b3d36de811a
Simple example to get an estimation of the attitude thought a 9DOF IMU and the Kalman filter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bclaus | 0:4b3d36de811a | 1 | #include "mbed.h" |
bclaus | 0:4b3d36de811a | 2 | #include "L3GD20.h" |
bclaus | 0:4b3d36de811a | 3 | #include "LSM303DLHC.h" |
capriele | 1:ba2d31e3112d | 4 | #include "math.h" |
capriele | 1:ba2d31e3112d | 5 | #include "arm_math.h" |
capriele | 1:ba2d31e3112d | 6 | #include "sensfusion9.h" |
capriele | 1:ba2d31e3112d | 7 | L3GD20 gyro(D14, D15); |
bclaus | 0:4b3d36de811a | 8 | Serial debug(USBTX,USBRX); |
capriele | 1:ba2d31e3112d | 9 | LSM303DLHC compass(D14, D15); |
bclaus | 0:4b3d36de811a | 10 | |
capriele | 1:ba2d31e3112d | 11 | float ax, ay, az; |
capriele | 1:ba2d31e3112d | 12 | float mx, my, mz; |
capriele | 1:ba2d31e3112d | 13 | float gx, gy, gz; |
capriele | 1:ba2d31e3112d | 14 | float roll, pitch, yaw; |
capriele | 1:ba2d31e3112d | 15 | Ticker kalmanTimer; |
capriele | 1:ba2d31e3112d | 16 | float dt = 0.01; |
bclaus | 0:4b3d36de811a | 17 | |
capriele | 1:ba2d31e3112d | 18 | void attitudeUpdate(void) { |
capriele | 1:ba2d31e3112d | 19 | } |
bclaus | 0:4b3d36de811a | 20 | |
capriele | 1:ba2d31e3112d | 21 | int main() { |
capriele | 1:ba2d31e3112d | 22 | //inizializzo filtro di kalman |
capriele | 1:ba2d31e3112d | 23 | sensfusion9Init(); |
capriele | 1:ba2d31e3112d | 24 | //kalmanTimer.attach(&attitudeUpdate, dt); |
capriele | 1:ba2d31e3112d | 25 | |
capriele | 1:ba2d31e3112d | 26 | debug.format(8, Serial::None, 1); |
bclaus | 0:4b3d36de811a | 27 | debug.baud(115200); |
capriele | 1:ba2d31e3112d | 28 | |
capriele | 1:ba2d31e3112d | 29 | //lunghezza asta |
capriele | 1:ba2d31e3112d | 30 | float l = 1.0f; |
bclaus | 0:4b3d36de811a | 31 | while(1) { |
capriele | 1:ba2d31e3112d | 32 | compass.read(&ax, &ay, &az, &mx, &my, &mz); |
capriele | 1:ba2d31e3112d | 33 | gyro.read(&gx, &gy, &gz); |
capriele | 1:ba2d31e3112d | 34 | sensfusion9UpdateQ(gx+(ax/l)*dt, gy+(ay/l)*dt, gz+(az/l)*dt, ax, ay, az, mx, my, mz, dt); |
capriele | 1:ba2d31e3112d | 35 | sensfusion9GetEulerRPY(&roll, &pitch, &yaw); |
capriele | 1:ba2d31e3112d | 36 | //debug.printf("%.10f %.10f %.10f\n\r", mx, my, mz); |
capriele | 1:ba2d31e3112d | 37 | //debug.printf("a %.4f %.4f %.4f m %.4f %.4f %.4f m %.4f %.4f %.4f R:%.4f P:%.4f Y:%.4f\n\r",ax,ay,az,mx,my,mz,gx,gy,gz,roll, pitch, yaw); |
capriele | 1:ba2d31e3112d | 38 | //debug.printf("R:%.4f P:%.4f Y:%.4f\n\r", roll, pitch, yaw); |
capriele | 1:ba2d31e3112d | 39 | debug.printf("%.4f %.4f %.4f\n\r", roll, pitch+3, yaw); |
capriele | 1:ba2d31e3112d | 40 | wait(dt); |
bclaus | 0:4b3d36de811a | 41 | } |
capriele | 1:ba2d31e3112d | 42 | } |