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