HAVRM mine / Mbed 2 deprecated Nucleo_L3GD20_MMA7361_Kalman

kalman filter using L3GD20 and MMA7361. But this program has about 0.5s delay

Dependencies:   L3GD20 MMA7361 mbed

main.cpp@0:e60a2d5cccc2, 2016-09-28 (annotated)

Committer:
hirokimineshita
Date:
Wed Sep 28 11:12:37 2016 +0000
Revision:
0:e60a2d5cccc2
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:e60a2d5cccc2 1 #include "mbed.h"
hirokimineshita 0:e60a2d5cccc2 2 #include "l3gd20.h"
hirokimineshita 0:e60a2d5cccc2 3 #include "mma7361.h"
hirokimineshita 0:e60a2d5cccc2 4 #include "kalman.h"
hirokimineshita 0:e60a2d5cccc2 5 #include "binary.h"
hirokimineshita 0:e60a2d5cccc2 6
hirokimineshita 0:e60a2d5cccc2 7 l3gd20 a;
hirokimineshita 0:e60a2d5cccc2 8 mma7361 b;
hirokimineshita 0:e60a2d5cccc2 9 Kalman c;
hirokimineshita 0:e60a2d5cccc2 10 DigitalOut myled(LED1);
hirokimineshita 0:e60a2d5cccc2 11 Serial pc(USBTX,USBRX);
hirokimineshita 0:e60a2d5cccc2 12 DigitalIn myb (USER_BUTTON);
hirokimineshita 0:e60a2d5cccc2 13 Timer t;
hirokimineshita 0:e60a2d5cccc2 14
hirokimineshita 0:e60a2d5cccc2 15 int main() {
hirokimineshita 0:e60a2d5cccc2 16 int us,d1,d2;
hirokimineshita 0:e60a2d5cccc2 17 myled=1;
hirokimineshita 0:e60a2d5cccc2 18 while(1) {
hirokimineshita 0:e60a2d5cccc2 19 if(myb==0){
hirokimineshita 0:e60a2d5cccc2 20 break;
hirokimineshita 0:e60a2d5cccc2 21 }
hirokimineshita 0:e60a2d5cccc2 22 }
hirokimineshita 0:e60a2d5cccc2 23
hirokimineshita 0:e60a2d5cccc2 24 myled=0;
hirokimineshita 0:e60a2d5cccc2 25 wait(1);
hirokimineshita 0:e60a2d5cccc2 26 while(a.conect());
hirokimineshita 0:e60a2d5cccc2 27 pc.printf("success\n\r");
hirokimineshita 0:e60a2d5cccc2 28 a.s_write_8(CTRL_REG1,b0001111);
hirokimineshita 0:e60a2d5cccc2 29 a.s_write_8(CTRL_REG5,b0000000);
hirokimineshita 0:e60a2d5cccc2 30 b.clear_mat();
hirokimineshita 0:e60a2d5cccc2 31 int x,z,fdata;
hirokimineshita 0:e60a2d5cccc2 32 int16_t data;
hirokimineshita 0:e60a2d5cccc2 33 b.set_1st_bit(&x,x_axis,0);
hirokimineshita 0:e60a2d5cccc2 34 b.set_1st_bit(&z,z_axis,1);
hirokimineshita 0:e60a2d5cccc2 35 float kx,gtheta,dt,atheta,xt,om,fom,iom;
hirokimineshita 0:e60a2d5cccc2 36 c.setAngle(0);
hirokimineshita 0:e60a2d5cccc2 37 atheta=0;
hirokimineshita 0:e60a2d5cccc2 38 fdata=0;
hirokimineshita 0:e60a2d5cccc2 39 fom=0;
hirokimineshita 0:e60a2d5cccc2 40 iom=0;
hirokimineshita 0:e60a2d5cccc2 41 gtheta=0;
hirokimineshita 0:e60a2d5cccc2 42 while(1){
hirokimineshita 0:e60a2d5cccc2 43 wait_ms(1);
hirokimineshita 0:e60a2d5cccc2 44 d1=a.read_8(OUT_Z_H);
hirokimineshita 0:e60a2d5cccc2 45 d2=a.read_8(OUT_Z_L);
hirokimineshita 0:e60a2d5cccc2 46 data=(d1<<8)|d2;
hirokimineshita 0:e60a2d5cccc2 47 b.low_pass_filter(&x,x_axis);
hirokimineshita 0:e60a2d5cccc2 48 xt=b.bit_to_g(x,x_axis);
hirokimineshita 0:e60a2d5cccc2 49 xt=xt*981/100;
hirokimineshita 0:e60a2d5cccc2 50 om=sqrt(abs(xt*1000/37));
hirokimineshita 0:e60a2d5cccc2 51 om=b.rad_to_deg(om);
hirokimineshita 0:e60a2d5cccc2 52 if(om/data<0){
hirokimineshita 0:e60a2d5cccc2 53 om*=-1;
hirokimineshita 0:e60a2d5cccc2 54 }
hirokimineshita 0:e60a2d5cccc2 55 iom=iom*0.9+om*0.1;
hirokimineshita 0:e60a2d5cccc2 56 /*b.low_pass_filter(&z,z_axis);
hirokimineshita 0:e60a2d5cccc2 57 atheta=b.rotate(&x,x_axis,&z,z_axis);
hirokimineshita 0:e60a2d5cccc2 58 atheta=b.rad_to_deg(atheta);*/
hirokimineshita 0:e60a2d5cccc2 59 t.stop();
hirokimineshita 0:e60a2d5cccc2 60 us=t.read_us();
hirokimineshita 0:e60a2d5cccc2 61 t.reset();
hirokimineshita 0:e60a2d5cccc2 62 t.start();
hirokimineshita 0:e60a2d5cccc2 63 dt=us*0.000001;
hirokimineshita 0:e60a2d5cccc2 64 gtheta+=(data+fdata)*0.00875*dt*0.5;
hirokimineshita 0:e60a2d5cccc2 65 atheta+=(om+fom)*dt*0.5;
hirokimineshita 0:e60a2d5cccc2 66 /*if(atheta/kx<0 && abs(kx)>70){
hirokimineshita 0:e60a2d5cccc2 67 atheta*=-1;
hirokimineshita 0:e60a2d5cccc2 68 }*/
hirokimineshita 0:e60a2d5cccc2 69 kx = c.getAngle(iom,gtheta,dt);
hirokimineshita 0:e60a2d5cccc2 70 pc.printf("%.3f,%.3f,%d\n\r",kx,atheta,data);
hirokimineshita 0:e60a2d5cccc2 71 //pc.printf("%.3f\n\r",om);
hirokimineshita 0:e60a2d5cccc2 72 fdata=data;
hirokimineshita 0:e60a2d5cccc2 73 fom=om;
hirokimineshita 0:e60a2d5cccc2 74 gtheta=kx;
hirokimineshita 0:e60a2d5cccc2 75 }
hirokimineshita 0:e60a2d5cccc2 76 }