Roger Weng
/
read_sensorFusion_data_BT
pure sensor fusion
main.cpp@1:81a146dffd7a, 2017-06-27 (annotated)
- Committer:
- roger5641
- Date:
- Tue Jun 27 07:37:44 2017 +0000
- Revision:
- 1:81a146dffd7a
- Parent:
- 0:56bfa7bd6f71
read_sensorFusion;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roger5641 | 0:56bfa7bd6f71 | 1 | #include "mbed.h" |
roger5641 | 0:56bfa7bd6f71 | 2 | #include "LSM9DS0.h" |
roger5641 | 0:56bfa7bd6f71 | 3 | #include <math.h> |
roger5641 | 0:56bfa7bd6f71 | 4 | |
roger5641 | 1:81a146dffd7a | 5 | #define pi 3.1415926 |
roger5641 | 1:81a146dffd7a | 6 | #define g 9.81 |
roger5641 | 1:81a146dffd7a | 7 | |
roger5641 | 0:56bfa7bd6f71 | 8 | // timer 1 |
roger5641 | 0:56bfa7bd6f71 | 9 | #define LOOPTIME 1000 // 1 ms |
roger5641 | 0:56bfa7bd6f71 | 10 | unsigned long lastMilli = 0; |
roger5641 | 0:56bfa7bd6f71 | 11 | // sampling time |
roger5641 | 0:56bfa7bd6f71 | 12 | float T = 0.001; |
roger5641 | 0:56bfa7bd6f71 | 13 | |
roger5641 | 0:56bfa7bd6f71 | 14 | Timer t; |
roger5641 | 0:56bfa7bd6f71 | 15 | |
roger5641 | 0:56bfa7bd6f71 | 16 | Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1 |
roger5641 | 0:56bfa7bd6f71 | 17 | |
roger5641 | 0:56bfa7bd6f71 | 18 | void init_uart(void); |
roger5641 | 0:56bfa7bd6f71 | 19 | void separate(void); |
roger5641 | 0:56bfa7bd6f71 | 20 | void uart_send(void); |
roger5641 | 0:56bfa7bd6f71 | 21 | |
roger5641 | 0:56bfa7bd6f71 | 22 | float lpf(float input, float output_old, float frequency); |
roger5641 | 0:56bfa7bd6f71 | 23 | |
roger5641 | 0:56bfa7bd6f71 | 24 | // uart send data |
roger5641 | 0:56bfa7bd6f71 | 25 | int display[6] = {0,0,0,0,0,0}; |
roger5641 | 0:56bfa7bd6f71 | 26 | char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte |
roger5641 | 0:56bfa7bd6f71 | 27 | |
roger5641 | 0:56bfa7bd6f71 | 28 | void init_sensor(void); |
roger5641 | 0:56bfa7bd6f71 | 29 | void real_sensor_value(void); |
roger5641 | 0:56bfa7bd6f71 | 30 | void sensor_fusion(void); |
roger5641 | 0:56bfa7bd6f71 | 31 | |
roger5641 | 0:56bfa7bd6f71 | 32 | LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6 |
roger5641 | 0:56bfa7bd6f71 | 33 | |
roger5641 | 0:56bfa7bd6f71 | 34 | int sensor_flag = 0; // sensor initial flag |
roger5641 | 0:56bfa7bd6f71 | 35 | int sensor_count = 0; |
roger5641 | 0:56bfa7bd6f71 | 36 | |
roger5641 | 1:81a146dffd7a | 37 | /*******************************Sensor***************************************/ |
roger5641 | 0:56bfa7bd6f71 | 38 | #define Gyro_gain_x 0.00113356568421052631578947368421 |
roger5641 | 0:56bfa7bd6f71 | 39 | #define Gyro_gain_y 0.00113356568421052631578947368421 |
roger5641 | 0:56bfa7bd6f71 | 40 | #define Gyro_gain_z 0.00113356568421052631578947368421 |
roger5641 | 1:81a146dffd7a | 41 | #define Acce_gain_x -0.00165873343713498511154419826254 //-9.81/(max-zero) |
roger5641 | 1:81a146dffd7a | 42 | #define Acce_gain_y -0.00217691902504394899561081096519 |
roger5641 | 1:81a146dffd7a | 43 | #define Acce_gain_z -0.00236798520156680310444239205579 |
roger5641 | 1:81a146dffd7a | 44 | |
roger5641 | 1:81a146dffd7a | 45 | /*******************************Sensor***************************************/ |
roger5641 | 1:81a146dffd7a | 46 | #define Gyro_gain_x 0.00113356568421052631578947368421 |
roger5641 | 1:81a146dffd7a | 47 | #define Gyro_gain_y 0.00113356568421052631578947368421 |
roger5641 | 1:81a146dffd7a | 48 | #define Gyro_gain_z 0.00113356568421052631578947368421 |
roger5641 | 1:81a146dffd7a | 49 | #define Acce_gain_x -0.00165873343713498511154419826254 //-9.81/(max-zero) |
roger5641 | 1:81a146dffd7a | 50 | #define Acce_gain_y -0.00217691902504394899561081096519 |
roger5641 | 1:81a146dffd7a | 51 | #define Acce_gain_z -0.00236798520156680310444239205579 |
roger5641 | 0:56bfa7bd6f71 | 52 | |
roger5641 | 0:56bfa7bd6f71 | 53 | // 宣告從sensor讀到的值要存入處理的變數 |
roger5641 | 0:56bfa7bd6f71 | 54 | float Gyro_axis_data[3]; // X, Y, Z axis |
roger5641 | 0:56bfa7bd6f71 | 55 | float Gyro_axis_data_f[3]; |
roger5641 | 0:56bfa7bd6f71 | 56 | float Gyro_axis_data_f_old[3]; |
roger5641 | 0:56bfa7bd6f71 | 57 | float Gyro_scale[3]; // scale = (data - zero) * gain |
roger5641 | 0:56bfa7bd6f71 | 58 | float Gyro_axis_zero[3] = {-3.7153254648333,-8.013402897667,-57.26524894}; |
roger5641 | 0:56bfa7bd6f71 | 59 | |
roger5641 | 0:56bfa7bd6f71 | 60 | float Acce_axis_data[3]; // X, Y, Z axis |
roger5641 | 0:56bfa7bd6f71 | 61 | float Acce_axis_data_f[3]; |
roger5641 | 0:56bfa7bd6f71 | 62 | float Acce_axis_data_f_old[3]; |
roger5641 | 0:56bfa7bd6f71 | 63 | float Acce_scale[3]; // scale = (data - zero) * gain |
roger5641 | 1:81a146dffd7a | 64 | float Acce_axis_zero[3] = {-840.873762375,-314.556930675,0.50990098975}; |
roger5641 | 1:81a146dffd7a | 65 | //-840.45049505 -395.32178215 4143.272277 |
roger5641 | 1:81a146dffd7a | 66 | //-841.2970297 4191.811881 1.3762376235 |
roger5641 | 1:81a146dffd7a | 67 | //5073.277228 -233.7920792 -0.356435644 |
roger5641 | 0:56bfa7bd6f71 | 68 | |
roger5641 | 0:56bfa7bd6f71 | 69 | // final sensor output value |
roger5641 | 0:56bfa7bd6f71 | 70 | float axm = 0; |
roger5641 | 0:56bfa7bd6f71 | 71 | float aym = 0; |
roger5641 | 0:56bfa7bd6f71 | 72 | float azm = 0; |
roger5641 | 0:56bfa7bd6f71 | 73 | float u1 = 0; |
roger5641 | 0:56bfa7bd6f71 | 74 | float u2 = 0; |
roger5641 | 0:56bfa7bd6f71 | 75 | float u3 = 0; |
roger5641 | 0:56bfa7bd6f71 | 76 | // new defined direction |
roger5641 | 0:56bfa7bd6f71 | 77 | float ax = 0; |
roger5641 | 0:56bfa7bd6f71 | 78 | float ay = 0; |
roger5641 | 0:56bfa7bd6f71 | 79 | float az = 0; |
roger5641 | 0:56bfa7bd6f71 | 80 | float w1 = 0; |
roger5641 | 0:56bfa7bd6f71 | 81 | float w2 = 0; |
roger5641 | 0:56bfa7bd6f71 | 82 | float w3 = 0; |
roger5641 | 0:56bfa7bd6f71 | 83 | // estimated state variables |
roger5641 | 0:56bfa7bd6f71 | 84 | float gs1_hat = 0; |
roger5641 | 0:56bfa7bd6f71 | 85 | float gs1_hat_old = 0; |
roger5641 | 0:56bfa7bd6f71 | 86 | float gs2_hat = 0; |
roger5641 | 0:56bfa7bd6f71 | 87 | float gs2_hat_old = 0; |
roger5641 | 0:56bfa7bd6f71 | 88 | float gs3_hat = 0; |
roger5641 | 0:56bfa7bd6f71 | 89 | float gs3_hat_old = 0; |
roger5641 | 0:56bfa7bd6f71 | 90 | // normalized variables |
roger5641 | 0:56bfa7bd6f71 | 91 | float n = 0; |
roger5641 | 0:56bfa7bd6f71 | 92 | float gs1_hat_n = 0; |
roger5641 | 0:56bfa7bd6f71 | 93 | float gs2_hat_n = 0; |
roger5641 | 0:56bfa7bd6f71 | 94 | float gs3_hat_n = 0; |
roger5641 | 1:81a146dffd7a | 95 | //general variables |
roger5641 | 1:81a146dffd7a | 96 | float gs1_hat_g = 0; |
roger5641 | 1:81a146dffd7a | 97 | float gs2_hat_g = 0; |
roger5641 | 1:81a146dffd7a | 98 | float gs3_hat_g = 0; |
roger5641 | 1:81a146dffd7a | 99 | |
roger5641 | 0:56bfa7bd6f71 | 100 | // bandwidth |
roger5641 | 0:56bfa7bd6f71 | 101 | float alpha = 6.28; // 1Hz |
roger5641 | 1:81a146dffd7a | 102 | float psi = 0.0; // pitch angle |
roger5641 | 0:56bfa7bd6f71 | 103 | |
roger5641 | 1:81a146dffd7a | 104 | //Sensor Fusion |
roger5641 | 1:81a146dffd7a | 105 | //****************theta Angle****************************// |
roger5641 | 1:81a146dffd7a | 106 | float theta = 0.0; // theta angle |
roger5641 | 1:81a146dffd7a | 107 | float theta_old = 0.0; |
roger5641 | 1:81a146dffd7a | 108 | float Itheta = 0.0; |
roger5641 | 1:81a146dffd7a | 109 | float Itheta_old = 0.0; |
roger5641 | 1:81a146dffd7a | 110 | float dtheta = 0.0; |
roger5641 | 1:81a146dffd7a | 111 | float dtheta_old = 0.0; |
roger5641 | 1:81a146dffd7a | 112 | float dtheta_f = 0.0; |
roger5641 | 1:81a146dffd7a | 113 | float dtheta_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 114 | |
roger5641 | 1:81a146dffd7a | 115 | //****************phi Angle****************************// |
roger5641 | 1:81a146dffd7a | 116 | float phi = 0.0; // theta angle |
roger5641 | 1:81a146dffd7a | 117 | float phi_old = 0.0; |
roger5641 | 1:81a146dffd7a | 118 | float Iphi = 0.0; |
roger5641 | 1:81a146dffd7a | 119 | float Iphi_old = 0.0; |
roger5641 | 1:81a146dffd7a | 120 | float dphi = 0.0; |
roger5641 | 1:81a146dffd7a | 121 | float dphi_old = 0.0; |
roger5641 | 1:81a146dffd7a | 122 | float dphi_f = 0.0; |
roger5641 | 1:81a146dffd7a | 123 | float dphi_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 124 | |
roger5641 | 1:81a146dffd7a | 125 | //****************roll Angle****************************// |
roger5641 | 1:81a146dffd7a | 126 | float roll = 0.0; |
roger5641 | 1:81a146dffd7a | 127 | float roll_old = 0.0; |
roger5641 | 1:81a146dffd7a | 128 | float roll_f = 0.0; |
roger5641 | 1:81a146dffd7a | 129 | float roll_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 130 | float Iroll = 0.0; // |
roger5641 | 1:81a146dffd7a | 131 | float Iroll_f = 0.0; |
roger5641 | 1:81a146dffd7a | 132 | float Iroll_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 133 | float droll = 0.0; //roll Angle |
roger5641 | 1:81a146dffd7a | 134 | float droll_old = 0.0; |
roger5641 | 1:81a146dffd7a | 135 | float droll_f = 0.0; |
roger5641 | 1:81a146dffd7a | 136 | float droll_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 137 | |
roger5641 | 1:81a146dffd7a | 138 | //float roll_1 = 0.0; |
roger5641 | 1:81a146dffd7a | 139 | //****************roll Angle without theta ****************************// |
roger5641 | 1:81a146dffd7a | 140 | float gama = 0.0; //Roll Angle |
roger5641 | 1:81a146dffd7a | 141 | float gama_old = 0.0; |
roger5641 | 1:81a146dffd7a | 142 | float gama_f = 0.0; |
roger5641 | 1:81a146dffd7a | 143 | float gama_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 144 | float Igama = 0.0; |
roger5641 | 1:81a146dffd7a | 145 | float Igama_f = 0.0; |
roger5641 | 1:81a146dffd7a | 146 | float Igama_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 147 | float dgama = 0.0; |
roger5641 | 1:81a146dffd7a | 148 | float dgama_old = 0.0; |
roger5641 | 1:81a146dffd7a | 149 | float dgama_f = 0.0; |
roger5641 | 1:81a146dffd7a | 150 | float dgama_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 151 | |
roger5641 | 1:81a146dffd7a | 152 | //****************pitch Angle****************************// |
roger5641 | 1:81a146dffd7a | 153 | float pitch = 0.0; //pitch Angle |
roger5641 | 1:81a146dffd7a | 154 | float pitch_old = 0.0; |
roger5641 | 1:81a146dffd7a | 155 | float pitch_f = 0.0; |
roger5641 | 1:81a146dffd7a | 156 | float pitch_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 157 | float Ipitch = 0.0; // |
roger5641 | 1:81a146dffd7a | 158 | float Ipitch_f = 0.0; |
roger5641 | 1:81a146dffd7a | 159 | float Ipitch_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 160 | float dpitch = 0.0; //pitch Angle |
roger5641 | 1:81a146dffd7a | 161 | float dpitch_old = 0.0; |
roger5641 | 1:81a146dffd7a | 162 | float dpitch_f = 0.0; |
roger5641 | 1:81a146dffd7a | 163 | float dpitch_f_old = 0.0; |
roger5641 | 1:81a146dffd7a | 164 | float pitch_0 = 0.0; // |
roger5641 | 1:81a146dffd7a | 165 | |
roger5641 | 0:56bfa7bd6f71 | 166 | int main() |
roger5641 | 0:56bfa7bd6f71 | 167 | { |
roger5641 | 0:56bfa7bd6f71 | 168 | t.start(); |
roger5641 | 0:56bfa7bd6f71 | 169 | |
roger5641 | 0:56bfa7bd6f71 | 170 | init_uart(); |
roger5641 | 0:56bfa7bd6f71 | 171 | init_sensor(); |
roger5641 | 0:56bfa7bd6f71 | 172 | |
roger5641 | 0:56bfa7bd6f71 | 173 | while(1) |
roger5641 | 0:56bfa7bd6f71 | 174 | { |
roger5641 | 0:56bfa7bd6f71 | 175 | // timer 1 |
roger5641 | 0:56bfa7bd6f71 | 176 | if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms |
roger5641 | 0:56bfa7bd6f71 | 177 | { |
roger5641 | 0:56bfa7bd6f71 | 178 | lastMilli = t.read_us(); |
roger5641 | 0:56bfa7bd6f71 | 179 | |
roger5641 | 0:56bfa7bd6f71 | 180 | // sensor initial start |
roger5641 | 0:56bfa7bd6f71 | 181 | if(sensor_flag == 0) |
roger5641 | 0:56bfa7bd6f71 | 182 | { |
roger5641 | 0:56bfa7bd6f71 | 183 | sensor_count++; |
roger5641 | 0:56bfa7bd6f71 | 184 | |
roger5641 | 0:56bfa7bd6f71 | 185 | if(sensor_count >= 50) |
roger5641 | 0:56bfa7bd6f71 | 186 | { |
roger5641 | 0:56bfa7bd6f71 | 187 | sensor_flag = 1; |
roger5641 | 0:56bfa7bd6f71 | 188 | sensor_count = 0; |
roger5641 | 0:56bfa7bd6f71 | 189 | } |
roger5641 | 0:56bfa7bd6f71 | 190 | } |
roger5641 | 0:56bfa7bd6f71 | 191 | else |
roger5641 | 0:56bfa7bd6f71 | 192 | { |
roger5641 | 0:56bfa7bd6f71 | 193 | real_sensor_value(); |
roger5641 | 0:56bfa7bd6f71 | 194 | sensor_fusion(); |
roger5641 | 0:56bfa7bd6f71 | 195 | uart_send(); |
roger5641 | 0:56bfa7bd6f71 | 196 | } |
roger5641 | 0:56bfa7bd6f71 | 197 | } |
roger5641 | 0:56bfa7bd6f71 | 198 | } // while(1) end |
roger5641 | 0:56bfa7bd6f71 | 199 | } |
roger5641 | 0:56bfa7bd6f71 | 200 | |
roger5641 | 0:56bfa7bd6f71 | 201 | int i = 0; |
roger5641 | 0:56bfa7bd6f71 | 202 | void uart_send(void) |
roger5641 | 0:56bfa7bd6f71 | 203 | { |
roger5641 | 0:56bfa7bd6f71 | 204 | // uart send data |
roger5641 | 0:56bfa7bd6f71 | 205 | display[0] = 100*pitch*180/3.1415926; |
roger5641 | 0:56bfa7bd6f71 | 206 | display[1] = 100*roll*180/3.1415926; |
roger5641 | 0:56bfa7bd6f71 | 207 | display[2] = 100*w3; |
roger5641 | 0:56bfa7bd6f71 | 208 | display[3] = 100*gs1_hat_n; |
roger5641 | 0:56bfa7bd6f71 | 209 | display[4] = 100*gs2_hat_n; |
roger5641 | 0:56bfa7bd6f71 | 210 | display[5] = 100*gs3_hat_n; |
roger5641 | 0:56bfa7bd6f71 | 211 | |
roger5641 | 0:56bfa7bd6f71 | 212 | separate(); |
roger5641 | 0:56bfa7bd6f71 | 213 | |
roger5641 | 0:56bfa7bd6f71 | 214 | int j = 0; |
roger5641 | 0:56bfa7bd6f71 | 215 | int k = 1; |
roger5641 | 0:56bfa7bd6f71 | 216 | while(k) |
roger5641 | 0:56bfa7bd6f71 | 217 | { |
roger5641 | 0:56bfa7bd6f71 | 218 | if(uart_1.writeable()) |
roger5641 | 0:56bfa7bd6f71 | 219 | { |
roger5641 | 0:56bfa7bd6f71 | 220 | uart_1.putc(num[i]); |
roger5641 | 0:56bfa7bd6f71 | 221 | i++; |
roger5641 | 0:56bfa7bd6f71 | 222 | j++; |
roger5641 | 0:56bfa7bd6f71 | 223 | } |
roger5641 | 0:56bfa7bd6f71 | 224 | |
roger5641 | 0:56bfa7bd6f71 | 225 | if(j>1) // send 2 bytes |
roger5641 | 0:56bfa7bd6f71 | 226 | { |
roger5641 | 0:56bfa7bd6f71 | 227 | k = 0; |
roger5641 | 0:56bfa7bd6f71 | 228 | j = 0; |
roger5641 | 0:56bfa7bd6f71 | 229 | } |
roger5641 | 0:56bfa7bd6f71 | 230 | } |
roger5641 | 0:56bfa7bd6f71 | 231 | |
roger5641 | 0:56bfa7bd6f71 | 232 | if(i>13) |
roger5641 | 0:56bfa7bd6f71 | 233 | i = 0; |
roger5641 | 0:56bfa7bd6f71 | 234 | } |
roger5641 | 0:56bfa7bd6f71 | 235 | |
roger5641 | 0:56bfa7bd6f71 | 236 | void real_sensor_value(void) |
roger5641 | 0:56bfa7bd6f71 | 237 | { |
roger5641 | 0:56bfa7bd6f71 | 238 | // sensor raw data |
roger5641 | 0:56bfa7bd6f71 | 239 | Gyro_axis_data[0] = sensor.readRawGyroX(); |
roger5641 | 0:56bfa7bd6f71 | 240 | Gyro_axis_data[1] = sensor.readRawGyroY(); |
roger5641 | 0:56bfa7bd6f71 | 241 | Gyro_axis_data[2] = sensor.readRawGyroZ(); |
roger5641 | 0:56bfa7bd6f71 | 242 | |
roger5641 | 0:56bfa7bd6f71 | 243 | Acce_axis_data[0] = sensor.readRawAccelX(); |
roger5641 | 0:56bfa7bd6f71 | 244 | Acce_axis_data[1] = sensor.readRawAccelY(); |
roger5641 | 0:56bfa7bd6f71 | 245 | Acce_axis_data[2] = sensor.readRawAccelZ(); |
roger5641 | 0:56bfa7bd6f71 | 246 | |
roger5641 | 0:56bfa7bd6f71 | 247 | // gyro filter |
roger5641 | 0:56bfa7bd6f71 | 248 | Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); |
roger5641 | 0:56bfa7bd6f71 | 249 | Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; |
roger5641 | 0:56bfa7bd6f71 | 250 | Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); |
roger5641 | 0:56bfa7bd6f71 | 251 | Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; |
roger5641 | 0:56bfa7bd6f71 | 252 | Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); |
roger5641 | 0:56bfa7bd6f71 | 253 | Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; |
roger5641 | 0:56bfa7bd6f71 | 254 | |
roger5641 | 0:56bfa7bd6f71 | 255 | // acce filter |
roger5641 | 0:56bfa7bd6f71 | 256 | Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); |
roger5641 | 0:56bfa7bd6f71 | 257 | Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; |
roger5641 | 0:56bfa7bd6f71 | 258 | Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); |
roger5641 | 0:56bfa7bd6f71 | 259 | Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; |
roger5641 | 0:56bfa7bd6f71 | 260 | Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); |
roger5641 | 0:56bfa7bd6f71 | 261 | Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; |
roger5641 | 0:56bfa7bd6f71 | 262 | |
roger5641 | 0:56bfa7bd6f71 | 263 | Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x; |
roger5641 | 0:56bfa7bd6f71 | 264 | Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y; |
roger5641 | 0:56bfa7bd6f71 | 265 | Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z; |
roger5641 | 0:56bfa7bd6f71 | 266 | |
roger5641 | 0:56bfa7bd6f71 | 267 | Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; |
roger5641 | 0:56bfa7bd6f71 | 268 | Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; |
roger5641 | 0:56bfa7bd6f71 | 269 | Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; |
roger5641 | 0:56bfa7bd6f71 | 270 | |
roger5641 | 0:56bfa7bd6f71 | 271 | // final 6 axis data |
roger5641 | 0:56bfa7bd6f71 | 272 | axm = Acce_scale[0]; |
roger5641 | 0:56bfa7bd6f71 | 273 | aym = Acce_scale[1]; |
roger5641 | 0:56bfa7bd6f71 | 274 | azm = Acce_scale[2]; |
roger5641 | 0:56bfa7bd6f71 | 275 | |
roger5641 | 0:56bfa7bd6f71 | 276 | u1 = Gyro_scale[0]; |
roger5641 | 0:56bfa7bd6f71 | 277 | u2 = Gyro_scale[1]; |
roger5641 | 0:56bfa7bd6f71 | 278 | u3 = Gyro_scale[2]; |
roger5641 | 0:56bfa7bd6f71 | 279 | |
roger5641 | 0:56bfa7bd6f71 | 280 | ax = axm; |
roger5641 | 0:56bfa7bd6f71 | 281 | ay = aym; |
roger5641 | 0:56bfa7bd6f71 | 282 | az = azm; |
roger5641 | 0:56bfa7bd6f71 | 283 | |
roger5641 | 0:56bfa7bd6f71 | 284 | w1 = u1; |
roger5641 | 0:56bfa7bd6f71 | 285 | w2 = u2; |
roger5641 | 0:56bfa7bd6f71 | 286 | w3 = u3; |
roger5641 | 0:56bfa7bd6f71 | 287 | } |
roger5641 | 0:56bfa7bd6f71 | 288 | |
roger5641 | 0:56bfa7bd6f71 | 289 | void init_uart() |
roger5641 | 0:56bfa7bd6f71 | 290 | { |
roger5641 | 0:56bfa7bd6f71 | 291 | uart_1.baud(115200); // 設定baudrate |
roger5641 | 0:56bfa7bd6f71 | 292 | } |
roger5641 | 0:56bfa7bd6f71 | 293 | |
roger5641 | 0:56bfa7bd6f71 | 294 | void separate(void) |
roger5641 | 0:56bfa7bd6f71 | 295 | { |
roger5641 | 0:56bfa7bd6f71 | 296 | num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 |
roger5641 | 0:56bfa7bd6f71 | 297 | num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 |
roger5641 | 0:56bfa7bd6f71 | 298 | num[4] = display[1] >> 8; |
roger5641 | 0:56bfa7bd6f71 | 299 | num[5] = display[1] & 0x00ff; |
roger5641 | 0:56bfa7bd6f71 | 300 | num[6] = display[2] >> 8; |
roger5641 | 0:56bfa7bd6f71 | 301 | num[7] = display[2] & 0x00ff; |
roger5641 | 0:56bfa7bd6f71 | 302 | num[8] = display[3] >> 8; |
roger5641 | 0:56bfa7bd6f71 | 303 | num[9] = display[3] & 0x00ff; |
roger5641 | 0:56bfa7bd6f71 | 304 | num[10] = display[4] >> 8; |
roger5641 | 0:56bfa7bd6f71 | 305 | num[11] = display[4] & 0x00ff; |
roger5641 | 0:56bfa7bd6f71 | 306 | num[12] = display[5] >> 8; |
roger5641 | 0:56bfa7bd6f71 | 307 | num[13] = display[5] & 0x00ff; |
roger5641 | 0:56bfa7bd6f71 | 308 | } |
roger5641 | 0:56bfa7bd6f71 | 309 | |
roger5641 | 0:56bfa7bd6f71 | 310 | void init_sensor(void) |
roger5641 | 0:56bfa7bd6f71 | 311 | { |
roger5641 | 0:56bfa7bd6f71 | 312 | sensor.begin(); |
roger5641 | 0:56bfa7bd6f71 | 313 | // sensor.begin() setting : |
roger5641 | 0:56bfa7bd6f71 | 314 | // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS, |
roger5641 | 0:56bfa7bd6f71 | 315 | // accel_scale aScl = A_SCALE_8G, |
roger5641 | 0:56bfa7bd6f71 | 316 | // mag_scale mScl = M_SCALE_8GS, |
roger5641 | 0:56bfa7bd6f71 | 317 | // gyro_odr gODR = G_ODR_760_BW_100, |
roger5641 | 0:56bfa7bd6f71 | 318 | // accel_odr aODR = A_ODR_800, |
roger5641 | 0:56bfa7bd6f71 | 319 | // mag_odr mODR = M_ODR_100); |
roger5641 | 0:56bfa7bd6f71 | 320 | } |
roger5641 | 0:56bfa7bd6f71 | 321 | |
roger5641 | 1:81a146dffd7a | 322 | void sensor_fusion(void) |
roger5641 | 1:81a146dffd7a | 323 | { |
roger5641 | 0:56bfa7bd6f71 | 324 | gs1_hat = lpf(ax + w3*ay/alpha - w2*az/alpha , gs1_hat_old , alpha); |
roger5641 | 0:56bfa7bd6f71 | 325 | gs1_hat_old = gs1_hat; |
roger5641 | 0:56bfa7bd6f71 | 326 | gs2_hat = lpf(-w3*ax/alpha + ay + w1*az/alpha , gs2_hat_old , alpha); |
roger5641 | 0:56bfa7bd6f71 | 327 | gs2_hat_old = gs2_hat; |
roger5641 | 0:56bfa7bd6f71 | 328 | gs3_hat = lpf(w2*ax/alpha - w1*ay/alpha + az , gs3_hat_old , alpha); |
roger5641 | 0:56bfa7bd6f71 | 329 | gs3_hat_old = gs3_hat; |
roger5641 | 0:56bfa7bd6f71 | 330 | |
roger5641 | 0:56bfa7bd6f71 | 331 | n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat); |
roger5641 | 1:81a146dffd7a | 332 | gs1_hat_n = (gs1_hat / n) * g; |
roger5641 | 1:81a146dffd7a | 333 | gs2_hat_n = (gs2_hat / n) * g; |
roger5641 | 1:81a146dffd7a | 334 | gs3_hat_n = (gs3_hat / n) * g; |
roger5641 | 0:56bfa7bd6f71 | 335 | |
roger5641 | 1:81a146dffd7a | 336 | gs1_hat_g = cos(theta)*gs1_hat_n-sin(theta)*gs2_hat_n; |
roger5641 | 1:81a146dffd7a | 337 | gs2_hat_g = cos(phi)*sin(theta)*gs1_hat_n + cos(phi)*cos(theta)*gs2_hat_n - sin(phi)*gs3_hat_n; |
roger5641 | 1:81a146dffd7a | 338 | gs3_hat_g = sin(phi)*sin(theta)*gs1_hat_n + sin(phi)*cos(theta)*gs2_hat_n - cos(phi)*gs3_hat_n; |
roger5641 | 1:81a146dffd7a | 339 | |
roger5641 | 1:81a146dffd7a | 340 | pitch = asin(gs1_hat_g/g); |
roger5641 | 1:81a146dffd7a | 341 | roll = atan(gs2_hat_g/gs3_hat_g); |
roger5641 | 1:81a146dffd7a | 342 | gama = atan(gs2_hat_n / gs3_hat_n);//+0.0157; |
roger5641 | 1:81a146dffd7a | 343 | pitch_0 = asin(-gs1_hat_n/g); |
roger5641 | 0:56bfa7bd6f71 | 344 | |
roger5641 | 0:56bfa7bd6f71 | 345 | pitch_f = lpf(pitch, pitch_f_old, 1.0); |
roger5641 | 0:56bfa7bd6f71 | 346 | pitch_f_old = pitch_f; |
roger5641 | 1:81a146dffd7a | 347 | Ipitch = Ipitch + pitch_f*T; |
roger5641 | 0:56bfa7bd6f71 | 348 | Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0); |
roger5641 | 0:56bfa7bd6f71 | 349 | Ipitch_f_old = Ipitch_f; |
roger5641 | 1:81a146dffd7a | 350 | dpitch = (pitch - dpitch_old)/T; |
roger5641 | 1:81a146dffd7a | 351 | dpitch_old = pitch; |
roger5641 | 0:56bfa7bd6f71 | 352 | dpitch_f = lpf(dpitch, dpitch_f_old, 1.0); |
roger5641 | 0:56bfa7bd6f71 | 353 | dpitch_f_old = dpitch_f; |
roger5641 | 0:56bfa7bd6f71 | 354 | |
roger5641 | 0:56bfa7bd6f71 | 355 | roll_f = lpf(roll, roll_f_old, 1.0); |
roger5641 | 0:56bfa7bd6f71 | 356 | roll_f_old = roll_f; |
roger5641 | 1:81a146dffd7a | 357 | Iroll = Iroll + roll_f*T; |
roger5641 | 0:56bfa7bd6f71 | 358 | Iroll_f = lpf(Iroll, Iroll_f_old, 18.0); |
roger5641 | 0:56bfa7bd6f71 | 359 | Iroll_f_old = Iroll_f; |
roger5641 | 1:81a146dffd7a | 360 | droll = (roll - droll_old)/T; |
roger5641 | 1:81a146dffd7a | 361 | droll_old = roll; |
roger5641 | 1:81a146dffd7a | 362 | droll_f = lpf(droll, droll_f_old, 1.0); |
roger5641 | 1:81a146dffd7a | 363 | droll_f_old = droll_f; |
roger5641 | 0:56bfa7bd6f71 | 364 | |
roger5641 | 1:81a146dffd7a | 365 | gama_f = lpf(gama, gama_f_old, 1.0); |
roger5641 | 1:81a146dffd7a | 366 | gama_f_old = gama_f; |
roger5641 | 1:81a146dffd7a | 367 | Igama = Igama + gama_f*T; |
roger5641 | 1:81a146dffd7a | 368 | Igama_f = lpf(Igama, Igama_f_old, 18.0); |
roger5641 | 1:81a146dffd7a | 369 | Igama_f_old = Igama_f; |
roger5641 | 1:81a146dffd7a | 370 | dgama = (gama - dgama_old)/T; |
roger5641 | 1:81a146dffd7a | 371 | dgama_old = gama; |
roger5641 | 1:81a146dffd7a | 372 | dgama_f = lpf(dgama, dgama_f_old, 1.0); |
roger5641 | 1:81a146dffd7a | 373 | dgama_f_old = dgama_f; |
roger5641 | 0:56bfa7bd6f71 | 374 | |
roger5641 | 0:56bfa7bd6f71 | 375 | } |
roger5641 | 0:56bfa7bd6f71 | 376 | |
roger5641 | 0:56bfa7bd6f71 | 377 | float lpf(float input, float output_old, float frequency) |
roger5641 | 0:56bfa7bd6f71 | 378 | { |
roger5641 | 0:56bfa7bd6f71 | 379 | float output = 0; |
roger5641 | 0:56bfa7bd6f71 | 380 | |
roger5641 | 0:56bfa7bd6f71 | 381 | output = (output_old + frequency*T*input) / (1 + frequency*T); |
roger5641 | 0:56bfa7bd6f71 | 382 | |
roger5641 | 0:56bfa7bd6f71 | 383 | return output; |
roger5641 | 0:56bfa7bd6f71 | 384 | } |