![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
version2
Fork of LSM9DS1_project by
main.cpp@2:c889fecf9afe, 2018-04-24 (annotated)
- Committer:
- middleyuan
- Date:
- Tue Apr 24 15:13:58 2018 +0000
- Revision:
- 2:c889fecf9afe
- Parent:
- 1:b1562831bbaf
- Child:
- 3:2c6b1c213ce0
SensorFusion
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beanmachine44 | 0:9632b831b6c1 | 1 | #include "mbed.h" |
beanmachine44 | 0:9632b831b6c1 | 2 | #include "LSM9DS1.h" |
middleyuan | 1:b1562831bbaf | 3 | #include "AS5145.h" |
beanmachine44 | 0:9632b831b6c1 | 4 | |
middleyuan | 2:c889fecf9afe | 5 | LSM9DS1 imu2(D5, D7); |
middleyuan | 2:c889fecf9afe | 6 | LSM9DS1 imu3(D3, D6); |
middleyuan | 1:b1562831bbaf | 7 | LSM9DS1 imu(D14, D15); |
middleyuan | 2:c889fecf9afe | 8 | AnalogIn sEMG(D13); |
middleyuan | 2:c889fecf9afe | 9 | AnalogIn sEMG2(D1); |
middleyuan | 2:c889fecf9afe | 10 | AnalogIn sEMG3(D0); |
middleyuan | 2:c889fecf9afe | 11 | AnalogIn sEMG4(PC_4); |
beanmachine44 | 0:9632b831b6c1 | 12 | Serial pc(USBTX, USBRX); |
middleyuan | 1:b1562831bbaf | 13 | Ticker timer1; |
middleyuan | 1:b1562831bbaf | 14 | Ticker timer2; |
beanmachine44 | 0:9632b831b6c1 | 15 | |
middleyuan | 1:b1562831bbaf | 16 | float T = 0.001; |
middleyuan | 1:b1562831bbaf | 17 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 18 | //function declaration |
middleyuan | 1:b1562831bbaf | 19 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 20 | void init_TIMER(void); |
middleyuan | 1:b1562831bbaf | 21 | void timer1_interrupt(void); |
middleyuan | 1:b1562831bbaf | 22 | void setup(void); |
middleyuan | 1:b1562831bbaf | 23 | void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha); |
middleyuan | 1:b1562831bbaf | 24 | float lpf(float input, float output_old, float frequency); |
middleyuan | 1:b1562831bbaf | 25 | void angle_fn(float x1_hat[3],float x2_hat[3]); |
middleyuan | 1:b1562831bbaf | 26 | void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]); |
middleyuan | 1:b1562831bbaf | 27 | void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]); |
middleyuan | 1:b1562831bbaf | 28 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 29 | // sensor data |
middleyuan | 1:b1562831bbaf | 30 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 31 | int16_t Gyro_axis_data[9] = {0}; // X, Y, Z axis |
middleyuan | 1:b1562831bbaf | 32 | int16_t Acce_axis_data[9] = {0}; // X, Y, Z axis |
middleyuan | 1:b1562831bbaf | 33 | float Gyro_axis_data_f[9] = {0}; |
middleyuan | 1:b1562831bbaf | 34 | float Gyro_axis_data_f_old[9] = {0}; |
middleyuan | 1:b1562831bbaf | 35 | float Acce_axis_data_f[9] = {0}; |
middleyuan | 1:b1562831bbaf | 36 | float Acce_axis_data_f_old[9] = {0}; |
middleyuan | 1:b1562831bbaf | 37 | float axm[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 38 | float aym[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 39 | float azm[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 40 | float w1[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 41 | float w2[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 42 | float w3[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 43 | //estimator |
middleyuan | 1:b1562831bbaf | 44 | float x1_hat[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 45 | float x2_hat[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 46 | float sinroll[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 47 | float cosroll[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 48 | float sinpitch[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 49 | float pitch_angle[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 50 | float roll_angle[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 51 | float yaw_dot[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 52 | float pitch_dot[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 53 | float pitch_double_dot[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 54 | float pitch_double_dot_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 55 | float pitch_double_dot_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 56 | float pitch_dot_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 57 | float axm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 58 | float axm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 59 | float w3aym_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 60 | float w3aym_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 61 | float w2azm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 62 | float w2azm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 63 | float aym_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 64 | float aym_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 65 | float w3axm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 66 | float w3axm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 67 | float w1azm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 68 | float w1azm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 69 | float azm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 70 | float azm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 71 | float w2axm_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 72 | float w2axm_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 73 | float w1aym_f[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 74 | float w1aym_f_old[3] = {0.0f}; |
middleyuan | 1:b1562831bbaf | 75 | //sEMG variable |
middleyuan | 1:b1562831bbaf | 76 | float emg_value[4] = {0.0f}; |
beanmachine44 | 0:9632b831b6c1 | 77 | |
beanmachine44 | 0:9632b831b6c1 | 78 | int main() |
beanmachine44 | 0:9632b831b6c1 | 79 | { |
middleyuan | 1:b1562831bbaf | 80 | pc.baud(230400); |
middleyuan | 1:b1562831bbaf | 81 | setup(); //Setup sensors |
middleyuan | 1:b1562831bbaf | 82 | AS5145_begin(); //begin encoder |
middleyuan | 1:b1562831bbaf | 83 | init_TIMER(); |
middleyuan | 1:b1562831bbaf | 84 | while (1) |
beanmachine44 | 0:9632b831b6c1 | 85 | { |
middleyuan | 2:c889fecf9afe | 86 | //pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096); |
middleyuan | 2:c889fecf9afe | 87 | wait(0.05); |
middleyuan | 1:b1562831bbaf | 88 | //pc.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]); |
middleyuan | 2:c889fecf9afe | 89 | pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096); |
middleyuan | 1:b1562831bbaf | 90 | //pc.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]); |
middleyuan | 1:b1562831bbaf | 91 | //pc.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]); |
middleyuan | 1:b1562831bbaf | 92 | //pc.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]); |
middleyuan | 1:b1562831bbaf | 93 | //pc.printf("position: %d,%d\r\n", position[0], position[1]); |
middleyuan | 1:b1562831bbaf | 94 | //pc.printf("roll_angle: %2f\r\n",roll_angle); |
middleyuan | 1:b1562831bbaf | 95 | //pc.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2); |
beanmachine44 | 0:9632b831b6c1 | 96 | } |
beanmachine44 | 0:9632b831b6c1 | 97 | } |
middleyuan | 1:b1562831bbaf | 98 | void setup() |
middleyuan | 1:b1562831bbaf | 99 | { |
middleyuan | 1:b1562831bbaf | 100 | imu.begin(); |
middleyuan | 1:b1562831bbaf | 101 | imu2.begin(); |
middleyuan | 1:b1562831bbaf | 102 | imu3.begin(); |
middleyuan | 1:b1562831bbaf | 103 | } |
middleyuan | 1:b1562831bbaf | 104 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 105 | // init_TIMER |
middleyuan | 1:b1562831bbaf | 106 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 107 | void init_TIMER(void) |
middleyuan | 1:b1562831bbaf | 108 | { |
middleyuan | 1:b1562831bbaf | 109 | timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) |
middleyuan | 1:b1562831bbaf | 110 | timer2.attach_us(&ReadValue, 1000);//1ms interrupt period (1000 Hz) |
middleyuan | 1:b1562831bbaf | 111 | } |
middleyuan | 1:b1562831bbaf | 112 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 113 | // timer1_interrupt |
middleyuan | 1:b1562831bbaf | 114 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 115 | void timer1_interrupt(void) |
middleyuan | 1:b1562831bbaf | 116 | { |
middleyuan | 1:b1562831bbaf | 117 | int i; |
middleyuan | 1:b1562831bbaf | 118 | imu.readAccel(); |
middleyuan | 1:b1562831bbaf | 119 | imu.readGyro(); |
middleyuan | 1:b1562831bbaf | 120 | imu2.readAccel(); |
middleyuan | 1:b1562831bbaf | 121 | imu2.readGyro(); |
middleyuan | 1:b1562831bbaf | 122 | imu3.readAccel(); |
middleyuan | 1:b1562831bbaf | 123 | imu3.readGyro(); |
middleyuan | 1:b1562831bbaf | 124 | // sensor raw data |
middleyuan | 1:b1562831bbaf | 125 | Acce_axis_data[0] = imu.ax*Acce_gain_x; |
middleyuan | 1:b1562831bbaf | 126 | Acce_axis_data[1] = imu.ay*Acce_gain_y; |
middleyuan | 1:b1562831bbaf | 127 | Acce_axis_data[2] = imu.az*Acce_gain_z; |
middleyuan | 1:b1562831bbaf | 128 | Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2; |
middleyuan | 1:b1562831bbaf | 129 | Acce_axis_data[4] = imu2.az*Acce_gain_y_2; |
middleyuan | 1:b1562831bbaf | 130 | Acce_axis_data[5] = imu2.ay*Acce_gain_z_2; |
middleyuan | 1:b1562831bbaf | 131 | Acce_axis_data[6] = -imu3.ax*Acce_gain_x_2; |
middleyuan | 1:b1562831bbaf | 132 | Acce_axis_data[7] = -imu3.az*Acce_gain_y_2; |
middleyuan | 1:b1562831bbaf | 133 | Acce_axis_data[8] = -imu3.ay*Acce_gain_z_2; |
middleyuan | 1:b1562831bbaf | 134 | |
middleyuan | 1:b1562831bbaf | 135 | Gyro_axis_data[0] = imu.gx*Gyro_gain_x; |
middleyuan | 1:b1562831bbaf | 136 | Gyro_axis_data[1] = imu.gy*Gyro_gain_y; |
middleyuan | 1:b1562831bbaf | 137 | Gyro_axis_data[2] = imu.gz*Gyro_gain_z; |
middleyuan | 1:b1562831bbaf | 138 | Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2; |
middleyuan | 1:b1562831bbaf | 139 | Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2; |
middleyuan | 1:b1562831bbaf | 140 | Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2; |
middleyuan | 1:b1562831bbaf | 141 | Gyro_axis_data[6] = -imu3.gx*Gyro_gain_x_2; |
middleyuan | 1:b1562831bbaf | 142 | Gyro_axis_data[7] = -imu3.gz*Gyro_gain_y_2; |
middleyuan | 1:b1562831bbaf | 143 | Gyro_axis_data[8] = -imu3.gy*Gyro_gain_z_2; |
middleyuan | 1:b1562831bbaf | 144 | |
middleyuan | 1:b1562831bbaf | 145 | for(i=0;i<9;i++) |
middleyuan | 1:b1562831bbaf | 146 | { |
middleyuan | 1:b1562831bbaf | 147 | Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15); |
middleyuan | 1:b1562831bbaf | 148 | Acce_axis_data_f_old[i] = Acce_axis_data_f[i]; |
middleyuan | 1:b1562831bbaf | 149 | Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15); |
middleyuan | 1:b1562831bbaf | 150 | Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i]; |
middleyuan | 1:b1562831bbaf | 151 | } |
middleyuan | 1:b1562831bbaf | 152 | |
middleyuan | 1:b1562831bbaf | 153 | axm[0] = Acce_axis_data_f[0]; |
middleyuan | 1:b1562831bbaf | 154 | aym[0] = Acce_axis_data_f[1]; |
middleyuan | 1:b1562831bbaf | 155 | azm[0] = Acce_axis_data_f[2]; |
middleyuan | 1:b1562831bbaf | 156 | w1[0] = Gyro_axis_data_f[0]; |
middleyuan | 1:b1562831bbaf | 157 | w2[0] = Gyro_axis_data_f[1]; |
middleyuan | 1:b1562831bbaf | 158 | w3[0] = Gyro_axis_data_f[2]; |
middleyuan | 1:b1562831bbaf | 159 | axm[1] = Acce_axis_data_f[3]; |
middleyuan | 1:b1562831bbaf | 160 | aym[1] = Acce_axis_data_f[4]; |
middleyuan | 1:b1562831bbaf | 161 | azm[1] = Acce_axis_data_f[5]; |
middleyuan | 1:b1562831bbaf | 162 | w1[1] = Gyro_axis_data_f[3]; |
middleyuan | 1:b1562831bbaf | 163 | w2[1] = Gyro_axis_data_f[4]; |
middleyuan | 1:b1562831bbaf | 164 | w3[1] = Gyro_axis_data_f[5]; |
middleyuan | 1:b1562831bbaf | 165 | axm[2] = Acce_axis_data_f[6]; |
middleyuan | 1:b1562831bbaf | 166 | aym[2] = Acce_axis_data_f[7]; |
middleyuan | 1:b1562831bbaf | 167 | azm[2] = Acce_axis_data_f[8]; |
middleyuan | 1:b1562831bbaf | 168 | w1[2] = Gyro_axis_data_f[6]; |
middleyuan | 1:b1562831bbaf | 169 | w2[2] = Gyro_axis_data_f[7]; |
middleyuan | 1:b1562831bbaf | 170 | w3[2] = Gyro_axis_data_f[8]; |
middleyuan | 1:b1562831bbaf | 171 | |
middleyuan | 1:b1562831bbaf | 172 | |
middleyuan | 2:c889fecf9afe | 173 | estimator(axm,aym,azm,w3,w2,w1,120); |
middleyuan | 1:b1562831bbaf | 174 | angle_fn(x1_hat,x2_hat); |
middleyuan | 1:b1562831bbaf | 175 | pitch_dot_fn(w3,w2,w1,sinroll,cosroll); |
middleyuan | 1:b1562831bbaf | 176 | pitch_double_dot_fn(pitch_dot,pitch_dot_old); |
middleyuan | 1:b1562831bbaf | 177 | |
middleyuan | 1:b1562831bbaf | 178 | for(i=0;i<3;i++) |
middleyuan | 1:b1562831bbaf | 179 | { |
middleyuan | 1:b1562831bbaf | 180 | pitch_dot_old[i] = pitch_dot[i]; |
middleyuan | 1:b1562831bbaf | 181 | } |
middleyuan | 1:b1562831bbaf | 182 | emg_value[0] = sEMG.read(); |
middleyuan | 1:b1562831bbaf | 183 | emg_value[1] = sEMG2.read(); |
middleyuan | 1:b1562831bbaf | 184 | emg_value[2] = sEMG3.read(); |
middleyuan | 1:b1562831bbaf | 185 | emg_value[3] = sEMG4.read(); |
middleyuan | 1:b1562831bbaf | 186 | } |
middleyuan | 1:b1562831bbaf | 187 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 188 | // estimator |
middleyuan | 1:b1562831bbaf | 189 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 190 | void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha) |
middleyuan | 1:b1562831bbaf | 191 | { |
middleyuan | 1:b1562831bbaf | 192 | int i; |
middleyuan | 1:b1562831bbaf | 193 | for(i=0;i<3;i++) |
middleyuan | 1:b1562831bbaf | 194 | { |
middleyuan | 1:b1562831bbaf | 195 | axm_f[i] = lpf(axm[i],axm_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 196 | axm_f_old[i] = axm_f[i]; |
middleyuan | 1:b1562831bbaf | 197 | w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 198 | w3aym_f_old[i] = w3aym_f[i]; |
middleyuan | 1:b1562831bbaf | 199 | w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 200 | w2azm_f_old[i] = w2azm_f[i]; |
middleyuan | 1:b1562831bbaf | 201 | aym_f[i] = lpf(aym[i],aym_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 202 | aym_f_old[i] = aym_f[i]; |
middleyuan | 1:b1562831bbaf | 203 | w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 204 | w3axm_f_old[i] = w3axm_f[i]; |
middleyuan | 1:b1562831bbaf | 205 | w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha); |
middleyuan | 1:b1562831bbaf | 206 | w1azm_f_old[i] = w1azm_f[i]; |
middleyuan | 1:b1562831bbaf | 207 | |
middleyuan | 1:b1562831bbaf | 208 | x1_hat[i] = axm_f[i] + w3aym_f[i]/alpha - w2azm_f[i]/alpha; |
middleyuan | 1:b1562831bbaf | 209 | x2_hat[i] = -w3axm_f[i]/alpha + aym_f[i] + w1azm_f[i]/alpha; |
middleyuan | 1:b1562831bbaf | 210 | } |
middleyuan | 1:b1562831bbaf | 211 | |
middleyuan | 1:b1562831bbaf | 212 | } |
middleyuan | 1:b1562831bbaf | 213 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 214 | // angle_fn |
middleyuan | 1:b1562831bbaf | 215 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 216 | void angle_fn(float x1_hat[3],float x2_hat[3]) |
middleyuan | 1:b1562831bbaf | 217 | { |
middleyuan | 1:b1562831bbaf | 218 | int i; |
middleyuan | 1:b1562831bbaf | 219 | for(i=0;i<3;i++) |
middleyuan | 1:b1562831bbaf | 220 | { |
middleyuan | 1:b1562831bbaf | 221 | sinroll[i] = x2_hat[i]*(-0.1020); |
middleyuan | 1:b1562831bbaf | 222 | if(sinroll[i] >= 1.0f) |
middleyuan | 1:b1562831bbaf | 223 | { |
middleyuan | 1:b1562831bbaf | 224 | sinroll[i] = 1.0; |
middleyuan | 1:b1562831bbaf | 225 | cosroll[i] = 0.0; |
middleyuan | 1:b1562831bbaf | 226 | } |
middleyuan | 1:b1562831bbaf | 227 | else if(sinroll[i] <= -1.0f) |
middleyuan | 1:b1562831bbaf | 228 | { |
middleyuan | 1:b1562831bbaf | 229 | sinroll[i] = -1.0; |
middleyuan | 1:b1562831bbaf | 230 | cosroll[i] = 0.0; |
middleyuan | 1:b1562831bbaf | 231 | } |
middleyuan | 1:b1562831bbaf | 232 | else cosroll[i] = sqrt(1-(sinroll[i]*sinroll[i])); |
middleyuan | 1:b1562831bbaf | 233 | roll_angle[i] = (asin(sinroll[i]))*180/pi; |
middleyuan | 1:b1562831bbaf | 234 | sinpitch[i] = x1_hat[i]*(0.1020f)/cosroll[i]; |
middleyuan | 1:b1562831bbaf | 235 | if(sinpitch[i] >= 1.0f) |
middleyuan | 1:b1562831bbaf | 236 | { |
middleyuan | 1:b1562831bbaf | 237 | sinpitch[i] = 1.0; |
middleyuan | 1:b1562831bbaf | 238 | } |
middleyuan | 1:b1562831bbaf | 239 | else if(sinpitch[i] <= -1.0f) |
middleyuan | 1:b1562831bbaf | 240 | { |
middleyuan | 1:b1562831bbaf | 241 | sinpitch[i] = -1.0; |
middleyuan | 1:b1562831bbaf | 242 | } |
middleyuan | 1:b1562831bbaf | 243 | |
middleyuan | 1:b1562831bbaf | 244 | pitch_angle[i] = (asin(sinpitch[i]))*180/pi; |
middleyuan | 1:b1562831bbaf | 245 | } |
middleyuan | 1:b1562831bbaf | 246 | } |
middleyuan | 1:b1562831bbaf | 247 | void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]) |
middleyuan | 1:b1562831bbaf | 248 | { |
middleyuan | 1:b1562831bbaf | 249 | int i; |
middleyuan | 1:b1562831bbaf | 250 | for(i=0;i<3;i++) |
middleyuan | 1:b1562831bbaf | 251 | { |
middleyuan | 1:b1562831bbaf | 252 | yaw_dot[i] = (w3[i]*cosroll[i] - w1[i]*sinroll[i])/cosroll[i]; |
middleyuan | 1:b1562831bbaf | 253 | pitch_dot[i] = w2[i] - yaw_dot[i]*sinroll[i]; |
middleyuan | 1:b1562831bbaf | 254 | } |
middleyuan | 1:b1562831bbaf | 255 | } |
middleyuan | 1:b1562831bbaf | 256 | void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]) |
middleyuan | 1:b1562831bbaf | 257 | { |
middleyuan | 1:b1562831bbaf | 258 | int i; |
middleyuan | 1:b1562831bbaf | 259 | for(i=0;i<3;i++) |
middleyuan | 1:b1562831bbaf | 260 | { |
middleyuan | 1:b1562831bbaf | 261 | pitch_double_dot[i] = (pitch_dot[i] - pitch_dot_old[i])/0.01f; |
middleyuan | 1:b1562831bbaf | 262 | pitch_double_dot_f[i] = lpf(pitch_double_dot[i],pitch_double_dot_f_old[i],30); |
middleyuan | 1:b1562831bbaf | 263 | pitch_double_dot_f_old[i] = pitch_double_dot_f[i]; |
middleyuan | 1:b1562831bbaf | 264 | } |
middleyuan | 1:b1562831bbaf | 265 | } |
middleyuan | 1:b1562831bbaf | 266 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 267 | // lpf |
middleyuan | 1:b1562831bbaf | 268 | /********************************************************************/ |
middleyuan | 1:b1562831bbaf | 269 | float lpf(float input, float output_old, float frequency) |
middleyuan | 1:b1562831bbaf | 270 | { |
middleyuan | 1:b1562831bbaf | 271 | float output = 0; |
middleyuan | 1:b1562831bbaf | 272 | |
middleyuan | 1:b1562831bbaf | 273 | output = (output_old + frequency*T*input) / (1 + frequency*T); |
middleyuan | 1:b1562831bbaf | 274 | |
middleyuan | 1:b1562831bbaf | 275 | return output; |
middleyuan | 1:b1562831bbaf | 276 | } |