Roger Weng / Mbed 2 deprecated read_sensorFusion_data_BT

pure sensor fusion

Dependencies:   LSM9DS0 mbed

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?

UserRevisionLine numberNew 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 }