20160814

Fork of SENSOR_FUSION by LDSC_Robotics_TAs

Committer:
adam_z
Date:
Thu Apr 28 09:08:53 2016 +0000
Revision:
0:e493567c21ac
This lib is in the form of global variables and functions instead of classes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:e493567c21ac 1 #include "SENSOR_FUSION.h"
adam_z 0:e493567c21ac 2 #include "math.h"
adam_z 0:e493567c21ac 3
adam_z 0:e493567c21ac 4 int conv_init;
adam_z 0:e493567c21ac 5
adam_z 0:e493567c21ac 6 float axm =0;
adam_z 0:e493567c21ac 7 float aym =0;
adam_z 0:e493567c21ac 8 float azm =0;
adam_z 0:e493567c21ac 9 float u1 = 0;
adam_z 0:e493567c21ac 10 float u2= 0;
adam_z 0:e493567c21ac 11 float u3= 0;
adam_z 0:e493567c21ac 12 float mx= 0;
adam_z 0:e493567c21ac 13 float my= 0;
adam_z 0:e493567c21ac 14 float mz= 0;
adam_z 0:e493567c21ac 15
adam_z 0:e493567c21ac 16 float axm_f= 0;
adam_z 0:e493567c21ac 17 float axm_f_old= 0;
adam_z 0:e493567c21ac 18 float u3aym_f= 0;
adam_z 0:e493567c21ac 19 float u3aym_f_old= 0;
adam_z 0:e493567c21ac 20 float u2azm_f= 0;
adam_z 0:e493567c21ac 21 float u2azm_f_old= 0;
adam_z 0:e493567c21ac 22
adam_z 0:e493567c21ac 23 float aym_f= 0;
adam_z 0:e493567c21ac 24 float aym_f_old= 0;
adam_z 0:e493567c21ac 25 float u3axm_f= 0;
adam_z 0:e493567c21ac 26 float u3axm_f_old= 0;
adam_z 0:e493567c21ac 27 float u1azm_f= 0;
adam_z 0:e493567c21ac 28 float u1azm_f_old= 0;
adam_z 0:e493567c21ac 29
adam_z 0:e493567c21ac 30 float u2axm_f= 0;
adam_z 0:e493567c21ac 31 float u2axm_f_old= 0;
adam_z 0:e493567c21ac 32 float u1aym_f= 0;
adam_z 0:e493567c21ac 33 float u1aym_f_old= 0;
adam_z 0:e493567c21ac 34 float azm_f= 0;
adam_z 0:e493567c21ac 35 float azm_f_old= 0;
adam_z 0:e493567c21ac 36
adam_z 0:e493567c21ac 37
adam_z 0:e493567c21ac 38 float x1_hat = 0;
adam_z 0:e493567c21ac 39 float x2_hat= 0;
adam_z 0:e493567c21ac 40 float x3_hat= 0;
adam_z 0:e493567c21ac 41 float cospitch= 0;
adam_z 0:e493567c21ac 42 float pitch_angle= 0;
adam_z 0:e493567c21ac 43 float roll_angle = 0;
adam_z 0:e493567c21ac 44
adam_z 0:e493567c21ac 45 float omega_phi= 0;
adam_z 0:e493567c21ac 46
adam_z 0:e493567c21ac 47 float lpf(float in, float out_old, float alpha, float TS) {
adam_z 0:e493567c21ac 48 float out;
adam_z 0:e493567c21ac 49
adam_z 0:e493567c21ac 50 out = (1. - alpha * TS) * out_old + alpha * TS*in;
adam_z 0:e493567c21ac 51 return out;
adam_z 0:e493567c21ac 52 }
adam_z 0:e493567c21ac 53
adam_z 0:e493567c21ac 54 void pitch_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_2,float alpha,float TS)
adam_z 0:e493567c21ac 55 {
adam_z 0:e493567c21ac 56 axm_f = lpf(a_xm,axm_f_old,alpha,TS);
adam_z 0:e493567c21ac 57 axm_f_old = axm_f;
adam_z 0:e493567c21ac 58 u3aym_f = lpf(u_3*a_ym,u3aym_f_old,alpha, TS);
adam_z 0:e493567c21ac 59 u3aym_f_old = u3aym_f;
adam_z 0:e493567c21ac 60 u2azm_f = lpf(u_2*a_zm,u2azm_f_old,alpha, TS);
adam_z 0:e493567c21ac 61 u2azm_f_old = u2azm_f;
adam_z 0:e493567c21ac 62
adam_z 0:e493567c21ac 63 x1_hat = axm_f + u3aym_f/alpha - u2azm_f/alpha;
adam_z 0:e493567c21ac 64 cospitch = sqrt(1-(x1_hat*0.1020*x1_hat*0.1020));
adam_z 0:e493567c21ac 65 pitch_angle = asin(x1_hat*0.1020);
adam_z 0:e493567c21ac 66 }
adam_z 0:e493567c21ac 67
adam_z 0:e493567c21ac 68 void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha, float TS)
adam_z 0:e493567c21ac 69 {
adam_z 0:e493567c21ac 70 aym_f = lpf(a_ym,aym_f_old,alpha, TS);
adam_z 0:e493567c21ac 71 aym_f_old = aym_f;
adam_z 0:e493567c21ac 72 u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha, TS);
adam_z 0:e493567c21ac 73 u3axm_f_old = u3axm_f;
adam_z 0:e493567c21ac 74 u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha, TS);
adam_z 0:e493567c21ac 75 u1azm_f_old = u1azm_f;
adam_z 0:e493567c21ac 76
adam_z 0:e493567c21ac 77 x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha;
adam_z 0:e493567c21ac 78
adam_z 0:e493567c21ac 79 }
adam_z 0:e493567c21ac 80
adam_z 0:e493567c21ac 81 void x3_hat_estimat(float a_xm,float a_ym,float a_zm,float u_2,float u_1,float alpha, float TS)
adam_z 0:e493567c21ac 82 {
adam_z 0:e493567c21ac 83 u2axm_f = lpf(u_2*a_xm,u2axm_f_old,alpha, TS);
adam_z 0:e493567c21ac 84 u2axm_f_old = u2axm_f;
adam_z 0:e493567c21ac 85 u1aym_f = lpf(u_1*a_ym,u1aym_f_old,alpha, TS);
adam_z 0:e493567c21ac 86 u1aym_f_old = u1aym_f;
adam_z 0:e493567c21ac 87 azm_f = lpf(a_zm,azm_f_old,alpha, TS);
adam_z 0:e493567c21ac 88 azm_f_old = azm_f;
adam_z 0:e493567c21ac 89
adam_z 0:e493567c21ac 90 x3_hat = u2axm_f/alpha - u1aym_f/alpha + azm_f;
adam_z 0:e493567c21ac 91
adam_z 0:e493567c21ac 92 roll_angle = asin(x3_hat*(-0.1012)/cospitch);
adam_z 0:e493567c21ac 93 omega_phi = u3;
adam_z 0:e493567c21ac 94
adam_z 0:e493567c21ac 95 }