20160814
Fork of SENSOR_FUSION by
SENSOR_FUSION.cpp@0:e493567c21ac, 2016-04-28 (annotated)
- 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?
User | Revision | Line number | New 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 | } |