Alberto Petrucci / Mbed 2 deprecated IMU-9DOF-KALMAN

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 brian claus

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?

UserRevisionLine numberNew 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 }