Chris LU / Mbed 2 deprecated Nucleo_ZTC_20160607_oldest

code of robot bike

Dependencies:   SDFileSystem mbed

Fork of Robot_Bicycle by Chris LU

SensorFusion.cpp@13:51ef67cd4fd6, 2017-05-17 (annotated)

Committer:
cpul5338
Date:
Wed May 17 15:22:29 2017 +0000
Revision:
13:51ef67cd4fd6
Parent:
0:830ddddc129f 
template

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:830ddddc129f 1 #include "SensorFusion.h"
YCTung 0:830ddddc129f 2 #include "SPI_9dSensor.h"
YCTung 0:830ddddc129f 3 #include "RobotBicycleConst.h"
YCTung 0:830ddddc129f 4 #include <math.h>
YCTung 0:830ddddc129f 5
YCTung 0:830ddddc129f 6 ///Gyro and Acce data
YCTung 0:830ddddc129f 7 float axm = 0.0f;
YCTung 0:830ddddc129f 8 float aym = 0.0f;
YCTung 0:830ddddc129f 9 float azm = 0.0f;
YCTung 0:830ddddc129f 10 float u1 = 0.0f;
YCTung 0:830ddddc129f 11 float u2 = 0.0f;
YCTung 0:830ddddc129f 12 float u3 = 0.0f;
YCTung 0:830ddddc129f 13 float mx = 0.0f;
YCTung 0:830ddddc129f 14 float my = 0.0f;
YCTung 0:830ddddc129f 15 float mz = 0.0f;
YCTung 0:830ddddc129f 16 float Ac[3] = {0.0f, 0.0f, 0.0f};
YCTung 0:830ddddc129f 17
YCTung 0:830ddddc129f 18 float axm_f = 0.0f;
YCTung 0:830ddddc129f 19 float axm_f_old = 0.0f;
YCTung 0:830ddddc129f 20 float u3aym_f = 0.0f;
YCTung 0:830ddddc129f 21 float u3aym_f_old = 0.0f;
YCTung 0:830ddddc129f 22 float u2azm_f = 0.0f;
YCTung 0:830ddddc129f 23 float u2azm_f_old = 0.0f;
YCTung 0:830ddddc129f 24
YCTung 0:830ddddc129f 25 float aym_f = 0.0f;
YCTung 0:830ddddc129f 26 float aym_f_old = 0.0f;
YCTung 0:830ddddc129f 27 float u3axm_f = 0.0f;
YCTung 0:830ddddc129f 28 float u3axm_f_old = 0.0f;
YCTung 0:830ddddc129f 29 float u1azm_f = 0.0f;
YCTung 0:830ddddc129f 30 float u1azm_f_old = 0.0f;
YCTung 0:830ddddc129f 31
YCTung 0:830ddddc129f 32 float azm_f = 0.0f;
YCTung 0:830ddddc129f 33 float azm_f_old = 0.0f;
YCTung 0:830ddddc129f 34 float u2axm_f = 0.0f;
YCTung 0:830ddddc129f 35 float u2axm_f_old = 0.0f;
YCTung 0:830ddddc129f 36 float u1aym_f = 0.0f;
YCTung 0:830ddddc129f 37 float u1aym_f_old = 0.0f;
YCTung 0:830ddddc129f 38
YCTung 0:830ddddc129f 39 float x1_hat = 0.0f;
YCTung 0:830ddddc129f 40 float x2_hat = 0.0f;
YCTung 0:830ddddc129f 41 float x3_hat = 0.0f;
YCTung 0:830ddddc129f 42 float sinroll = 0.0f;
YCTung 0:830ddddc129f 43 float cosroll = 0.0f;
YCTung 0:830ddddc129f 44 float roll_angle = 0.0f;
YCTung 0:830ddddc129f 45 float droll_angle= 0.0f;
YCTung 0:830ddddc129f 46 float droll_angle_old= 0.0f;
YCTung 0:830ddddc129f 47 float sinpitch = 0.0f;
YCTung 0:830ddddc129f 48 float cospitch = 0.0f;
YCTung 0:830ddddc129f 49 float yaw_ref = 0.0f;
YCTung 0:830ddddc129f 50 float yaw_angle = 0.0f;
YCTung 0:830ddddc129f 51 float yaw_angle_old = 0.0f;
YCTung 0:830ddddc129f 52 float dyaw_angle = 0.0f;
YCTung 0:830ddddc129f 53 float dyaw_angle_old = 0.0f;
YCTung 0:830ddddc129f 54
YCTung 0:830ddddc129f 55 ///magnetometer's data
YCTung 0:830ddddc129f 56 float mx_f = 0.0f;
YCTung 0:830ddddc129f 57 float mx_f_old = 0.0f;
YCTung 0:830ddddc129f 58 float u3my_f = 0.0f;
YCTung 0:830ddddc129f 59 float u3my_f_old = 0.0f;
YCTung 0:830ddddc129f 60 float u2mz_f = 0.0f;
YCTung 0:830ddddc129f 61 float u2mz_f_old = 0.0f;
YCTung 0:830ddddc129f 62
YCTung 0:830ddddc129f 63 float my_f = 0.0f;
YCTung 0:830ddddc129f 64 float my_f_old = 0.0f;
YCTung 0:830ddddc129f 65 float u3mx_f = 0.0f;
YCTung 0:830ddddc129f 66 float u3mx_f_old = 0.0f;
YCTung 0:830ddddc129f 67 float u1mz_f = 0.0f;
YCTung 0:830ddddc129f 68 float u1mz_f_old = 0.0f;
YCTung 0:830ddddc129f 69
YCTung 0:830ddddc129f 70 float mz_f = 0.0f;
YCTung 0:830ddddc129f 71 float mz_f_old = 0.0f;
YCTung 0:830ddddc129f 72 float u2mx_f = 0.0f;
YCTung 0:830ddddc129f 73 float u2mx_f_old = 0.0f;
YCTung 0:830ddddc129f 74 float u1my_f = 0.0f;
YCTung 0:830ddddc129f 75 float u1my_f_old = 0.0f;
YCTung 0:830ddddc129f 76
YCTung 0:830ddddc129f 77 float mx1_hat = 0.0f;
YCTung 0:830ddddc129f 78 float mx2_hat = 0.0f;
YCTung 0:830ddddc129f 79 float mx3_hat = 0.0f;
YCTung 0:830ddddc129f 80 float m_sinyaw = 0.0f;
YCTung 0:830ddddc129f 81 float m_cosyaw = 0.0f;
YCTung 0:830ddddc129f 82 float m_yaw_angle = 0.0f;
YCTung 0:830ddddc129f 83
YCTung 0:830ddddc129f 84 void get_9axis_data(unsigned char speed_state)
YCTung 0:830ddddc129f 85 {
YCTung 0:830ddddc129f 86 Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2];
YCTung 0:830ddddc129f 87 if(speed_state == 3) Ac[1] = (-1.0) * v_high * Gyro_scale[2];
YCTung 0:830ddddc129f 88 else if(speed_state == 2) Ac[1] = (-1.0) * v_low * Gyro_scale[2];
YCTung 0:830ddddc129f 89 else Ac[1] = 0.0;///(-1.0) * Vx * Gyro_scale[2];///
YCTung 0:830ddddc129f 90 Ac[2] = 0.0;
YCTung 0:830ddddc129f 91
YCTung 0:830ddddc129f 92 axm = Acce_scale[0] - Ac[0];
YCTung 0:830ddddc129f 93 aym = Acce_scale[1] - Ac[1];
YCTung 0:830ddddc129f 94 azm = Acce_scale[2];
YCTung 0:830ddddc129f 95 u1 = Gyro_scale[0];
YCTung 0:830ddddc129f 96 u2 = Gyro_scale[1];
YCTung 0:830ddddc129f 97 u3 = Gyro_scale[2];
YCTung 0:830ddddc129f 98 mx = Magn_scale[0];
YCTung 0:830ddddc129f 99 my = Magn_scale[1];
YCTung 0:830ddddc129f 100 mz = Magn_scale[2]*(-1);
YCTung 0:830ddddc129f 101 B_total = sqrt(mx*mx+my*my+mz*mz);
YCTung 0:830ddddc129f 102 }
YCTung 0:830ddddc129f 103
YCTung 0:830ddddc129f 104 float lpf(float input,float input_old,float frequency)
YCTung 0:830ddddc129f 105 {
YCTung 0:830ddddc129f 106 float output = 0;
YCTung 0:830ddddc129f 107 output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
YCTung 0:830ddddc129f 108 return output;
YCTung 0:830ddddc129f 109 }
YCTung 0:830ddddc129f 110
YCTung 0:830ddddc129f 111 void x1_hat_estimat(float a_xm,float a_ym,float a_zm,float u_3,float u_2,float alpha)
YCTung 0:830ddddc129f 112 {
YCTung 0:830ddddc129f 113 axm_f = lpf(a_xm,axm_f_old,alpha);
YCTung 0:830ddddc129f 114 axm_f_old = axm_f;
YCTung 0:830ddddc129f 115 u3aym_f = lpf(u_3*a_ym,u3aym_f_old,alpha);
YCTung 0:830ddddc129f 116 u3aym_f_old = u3aym_f;
YCTung 0:830ddddc129f 117 u2azm_f = lpf(u_2*a_zm,u2azm_f_old,alpha);
YCTung 0:830ddddc129f 118 u2azm_f_old = u2azm_f;
YCTung 0:830ddddc129f 119
YCTung 0:830ddddc129f 120 x1_hat = axm_f + u3aym_f/alpha - u2azm_f/alpha;
YCTung 0:830ddddc129f 121 }
YCTung 0:830ddddc129f 122
YCTung 0:830ddddc129f 123 void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
YCTung 0:830ddddc129f 124 {
YCTung 0:830ddddc129f 125 aym_f = lpf(a_ym,aym_f_old,alpha);
YCTung 0:830ddddc129f 126 aym_f_old = aym_f;
YCTung 0:830ddddc129f 127 u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha);
YCTung 0:830ddddc129f 128 u3axm_f_old = u3axm_f;
YCTung 0:830ddddc129f 129 u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha);
YCTung 0:830ddddc129f 130 u1azm_f_old = u1azm_f;
YCTung 0:830ddddc129f 131
YCTung 0:830ddddc129f 132 x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha;
YCTung 0:830ddddc129f 133 sinroll = x2_hat*(-0.1020);
YCTung 0:830ddddc129f 134 if(sinroll >= 1.0f)
YCTung 0:830ddddc129f 135 {
YCTung 0:830ddddc129f 136 sinroll = 1.0;
YCTung 0:830ddddc129f 137 cosroll = 0.0;
YCTung 0:830ddddc129f 138 }
YCTung 0:830ddddc129f 139 else if(sinroll <= -1.0f)
YCTung 0:830ddddc129f 140 {
YCTung 0:830ddddc129f 141 sinroll = -1.0;
YCTung 0:830ddddc129f 142 cosroll = 0.0;
YCTung 0:830ddddc129f 143 }
YCTung 0:830ddddc129f 144 else cosroll = sqrt(1-(sinroll*sinroll));
YCTung 0:830ddddc129f 145
YCTung 0:830ddddc129f 146 roll_angle = asin(sinroll);///By 312 Rotation
YCTung 0:830ddddc129f 147 }
YCTung 0:830ddddc129f 148
YCTung 0:830ddddc129f 149 void x3_hat_estimat(float a_xm,float a_ym,float a_zm,float u_2,float u_1,float alpha)
YCTung 0:830ddddc129f 150 {
YCTung 0:830ddddc129f 151 u2axm_f = lpf(u_2*a_xm,u2axm_f_old,alpha);
YCTung 0:830ddddc129f 152 u2axm_f_old = u2axm_f;
YCTung 0:830ddddc129f 153 u1aym_f = lpf(u_1*a_ym,u1aym_f_old,alpha);
YCTung 0:830ddddc129f 154 u1aym_f_old = u1aym_f;
YCTung 0:830ddddc129f 155 azm_f = lpf(a_zm,azm_f_old,alpha);
YCTung 0:830ddddc129f 156 azm_f_old = azm_f;
YCTung 0:830ddddc129f 157
YCTung 0:830ddddc129f 158 x3_hat = u2axm_f/alpha - u1aym_f/alpha + azm_f;
YCTung 0:830ddddc129f 159 }
YCTung 0:830ddddc129f 160
YCTung 0:830ddddc129f 161 void m_x1_hat(float m_x, float m_y,float m_z,float u_3,float u_2,float alpha)
YCTung 0:830ddddc129f 162 {
YCTung 0:830ddddc129f 163 mx_f = lpf(m_x,mx_f_old,alpha);
YCTung 0:830ddddc129f 164 mx_f_old = mx_f;
YCTung 0:830ddddc129f 165 u3my_f = lpf(u_3*m_y,u3my_f_old,alpha);
YCTung 0:830ddddc129f 166 u3my_f_old = u3my_f;
YCTung 0:830ddddc129f 167 u2mz_f = lpf(u_2*m_z,u2mz_f_old,alpha);
YCTung 0:830ddddc129f 168 u2mz_f_old = u2mz_f;
YCTung 0:830ddddc129f 169
YCTung 0:830ddddc129f 170 mx1_hat = (mx_f + u3my_f/alpha - u2mz_f/alpha)/B_total;
YCTung 0:830ddddc129f 171 }
YCTung 0:830ddddc129f 172
YCTung 0:830ddddc129f 173 void m_x2_hat(float m_x,float m_y,float m_z,float u_3,float u_1,float alpha)
YCTung 0:830ddddc129f 174 {
YCTung 0:830ddddc129f 175 my_f = lpf(m_y,my_f_old,alpha);
YCTung 0:830ddddc129f 176 my_f_old = my_f;
YCTung 0:830ddddc129f 177 u3mx_f = lpf(u_3*m_x,u3mx_f_old,alpha);
YCTung 0:830ddddc129f 178 u3mx_f_old = u3mx_f;
YCTung 0:830ddddc129f 179 u1mz_f = lpf(u_1*m_z,u1mz_f_old,alpha);
YCTung 0:830ddddc129f 180 u1mz_f_old = u1mz_f;
YCTung 0:830ddddc129f 181
YCTung 0:830ddddc129f 182 mx2_hat = (my_f - u3mx_f/alpha + u1mz_f/alpha)/B_total;
YCTung 0:830ddddc129f 183 }
YCTung 0:830ddddc129f 184
YCTung 0:830ddddc129f 185 void m_x3_hat(float m_x,float m_y,float m_z,float u_2,float u_1,float alpha)
YCTung 0:830ddddc129f 186 {
YCTung 0:830ddddc129f 187 u2mx_f = lpf(u_2*m_x,u2mx_f_old,alpha);
YCTung 0:830ddddc129f 188 u2mx_f_old = u2mx_f;
YCTung 0:830ddddc129f 189 u1my_f = lpf(u_1*m_y,u1my_f_old,alpha);
YCTung 0:830ddddc129f 190 u1my_f_old = u1my_f;
YCTung 0:830ddddc129f 191 mz_f = lpf(m_z,mz_f_old,alpha);
YCTung 0:830ddddc129f 192 mz_f_old = mz_f;
YCTung 0:830ddddc129f 193
YCTung 0:830ddddc129f 194 mx3_hat = (u2mx_f/alpha - u1my_f/alpha + mz_f)/B_total;
YCTung 0:830ddddc129f 195 }
YCTung 0:830ddddc129f 196
YCTung 0:830ddddc129f 197 void m_yaw_fusion(void)
YCTung 0:830ddddc129f 198 {
YCTung 0:830ddddc129f 199 float delta, delta_1, delta_2;
YCTung 0:830ddddc129f 200
YCTung 0:830ddddc129f 201 delta = Determinant((B_x*cospitch+B_y*sinpitch*sinroll), (B_y*cospitch-B_x*sinpitch*sinroll), (B_y*cosroll), (-B_x*cosroll));
YCTung 0:830ddddc129f 202 delta_1 = Determinant((mx1_hat+B_z*sinpitch*cosroll), (B_y*cospitch-B_x*sinpitch*sinroll), (mx2_hat-B_z*sinroll), (-B_x*cosroll));
YCTung 0:830ddddc129f 203 delta_2 = Determinant((B_x*cospitch+B_y*sinpitch*sinroll), (mx1_hat+B_z*sinpitch*cosroll), (B_y*cosroll), (mx2_hat-B_z*sinroll));
YCTung 0:830ddddc129f 204 ///B_y = 0
YCTung 0:830ddddc129f 205 // delta = Determinant((B_x*cospitch), (-B_x*sinpitch*sinroll), (0.0), (-B_x*cosroll));
YCTung 0:830ddddc129f 206 // delta_1 = Determinant((mx1_hat+B_z*sinpitch*cosroll), (-B_x*sinpitch*sinroll), (mx2_hat-B_z*sinroll), (-B_x*cosroll));
YCTung 0:830ddddc129f 207 // delta_2 = Determinant((B_x*cospitch), (mx1_hat+B_z*sinpitch*cosroll), (0.0), (mx2_hat-B_z*sinroll));
YCTung 0:830ddddc129f 208
YCTung 0:830ddddc129f 209 m_cosyaw = delta_1/delta;
YCTung 0:830ddddc129f 210 m_sinyaw = delta_2/delta;
YCTung 0:830ddddc129f 211 }
YCTung 0:830ddddc129f 212
YCTung 0:830ddddc129f 213 float Determinant(float x11, float x12, float x21, float x22)
YCTung 0:830ddddc129f 214 {
YCTung 0:830ddddc129f 215 return (x11*x22 - x12*x21);
YCTung 0:830ddddc129f 216 }
YCTung 0:830ddddc129f 217
YCTung 0:830ddddc129f 218 float absolute(float value)
YCTung 0:830ddddc129f 219 {
YCTung 0:830ddddc129f 220 if(value < 0)return -value;
YCTung 0:830ddddc129f 221 else return value;
YCTung 0:830ddddc129f 222 }
YCTung 0:830ddddc129f 223
YCTung 0:830ddddc129f 224 void Reset_data(void)
YCTung 0:830ddddc129f 225 {
YCTung 0:830ddddc129f 226 axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0;
YCTung 0:830ddddc129f 227 aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0;
YCTung 0:830ddddc129f 228 azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0;
YCTung 0:830ddddc129f 229 }