Roger Weng / Mbed 2 deprecated read_sensorFusion_data_BT

pure sensor fusion

Dependencies:   LSM9DS0 mbed

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?

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