hige dura
Dependencies: FatFileSystem mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // dot-HR EKF 00002 00003 #include "mbed.h" 00004 #include "SDFileSystem.h" 00005 #include "ADXL345_I2C.h" 00006 #include "ITG3200.h" 00007 #include "HMC5883L.h" 00008 00009 Serial pc(USBTX, USBRX); 00010 DigitalOut led1(LED1); 00011 DigitalOut led2(LED2); 00012 DigitalOut led3(LED3); 00013 DigitalOut led4(LED4); 00014 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board 00015 ADXL345_I2C accelerometer(p9, p10); 00016 ITG3200 gyro(p9, p10); 00017 HMC5883L compass(p9, p10); 00018 Serial xbee(p13, p14); 00019 DigitalIn stop(p20); 00020 PwmOut ESC1(p21); 00021 PwmOut ESC2(p22); 00022 PwmOut ESC3(p23); 00023 PwmOut ESC4(p24); 00024 PwmOut ESC5(p25); 00025 PwmOut ESC6(p26); 00026 Serial navi(p28, p27); // tx, rx 00027 DigitalOut navi_flag(p29); 00028 00029 #define pi 3.14159265 00030 00031 #define N 3 // phi, the, psi 00032 #define M 6 // The numbers of rotors 00033 #define L 5 // The numbers of state quantities (optimal regulator) 00034 #define chan 4 // The numbers of channels 00035 #define chan_buf 17 00036 #define N_LWMA 100 00037 00038 int preparing_acc(); 00039 double* calib(); 00040 double* RK4( double, double[N], double[N] ); 00041 double* func( double[N], double[N] ); 00042 double* LWMA( double[N] ); 00043 double* EKF_predict( double[N], double[N] ); 00044 double* EKF_correct( double[N], double[N], double[N] ); 00045 00046 // 0 1 2 00047 // [ accXn-1 accXn-2 ... ] 0 00048 // zBuf = [ accYn-1 accYn-2 ... ] 1 00049 // [ accZn-1 accZn-2 ... ] 2 00050 double z_buf[N][N_LWMA] = {0}; // For LWMA 00051 00052 double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF 00053 00054 int main(){ 00055 00056 pc.baud(921600); 00057 navi.baud(57600); 00058 xbee.baud(57600); 00059 00060 FILE *fp = fopen("/sd/sdtest.txt", "w"); 00061 if(fp == NULL) { 00062 error("Could not open file for write\n"); 00063 while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; } 00064 } 00065 00066 int correct_period = 100; 00067 int navi_period = 10; 00068 int xbee_period = 100; 00069 int correct_loop = 0; 00070 int navi_loop = 10; 00071 int xbee_loop = 100; 00072 00073 double dt_wait = 0.00043; 00074 //double dt_wait = 0.01; 00075 double dt = 0.01; 00076 double t = 0; 00077 00078 int bit_acc[N] = {0}; // Buffer of the accelerometer 00079 int get_mag[N] = {0}; // Buffer of the compass 00080 00081 // Calibration routine 00082 double calib_acc[N] = {0}; 00083 double calib_gyro[N] = {0}; 00084 //double compass_basis_rad = 0; 00085 00086 // Getting data 00087 double acc[N] = {0, 0, 1}; 00088 double acc0[N] = {0, 0, 1}; 00089 double d_acc[N] = {0}; 00090 double gyro_deg[N] = {0}; 00091 double gyro_rad[N] = {0}; 00092 //int mag[N] = {0}; 00093 00094 // Measurement algorithm 00095 //double angle_acc[2] = {0}; 00096 double angle_deg[N] = {0}; 00097 double angle_rad[N] = {0}; 00098 double zLWMA[N] = {0}; 00099 //double compass_rad = 0; 00100 //double compass_deg = 0; 00101 00102 // Gravity z 00103 for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; } 00104 00105 double* p_calib; 00106 double* p_RK4; 00107 double* p_EKF; 00108 double* p_zLWMA; 00109 00110 // Optimal regulator 00111 double state[L]; 00112 // Gain (q1=10, q2=10, q3=10, q4=10, q5=1) 00113 /* double Kr[M][L] = {{ 0, 1.826, 0, 1.984, 0.408}, 00114 {-1.581, 0.913,-1.718, 0.992,-0.408}, 00115 {-1.581,-0.913,-1.718,-0.992, 0.408}, 00116 { 0,-1.826, 0,-1.984,-0.408}, 00117 { 1.581,-0.913, 1.718,-0.992, 0.408}, 00118 { 1.581, 0.913, 1.718, 0.992,-0.408}}; 00119 */ 00120 // Gain (q1=100, q2=100, q3=1, q4=1, q5=1) 00121 /* double Kr[M][L] = {{ 0, 5.774, 0, 1.496, 0.408}, 00122 {-5, 2.887,-1.296, 0.748,-0.408}, 00123 {-5,-2.887,-1.296,-0.748, 0.408}, 00124 { 0,-5.774, 0,-1.496,-0.408}, 00125 { 5,-2.887, 1.296,-0.748, 0.408}, 00126 { 5, 2.887, 1.296, 0.748,-0.408}}; 00127 */ 00128 // Gain (q1=100, q2=100, q3=10, q4=10, q5=1) 00129 /* double Kr[M][L] = {{ 0, 5.774, 0, 2.288, 0.408}, 00130 {-5, 2.887,-1.982, 1.144,-0.408}, 00131 {-5,-2.887,-1.982,-1.144, 0.408}, 00132 { 0,-5.774, 0,-2.288,-0.408}, 00133 { 5,-2.887, 1.982,-1.144, 0.408}, 00134 { 5, 2.887, 1.982, 1.144,-0.408}}; 00135 */ 00136 // Gain (q1=100, q2=100, q3=50, q4=50, q5=1) 00137 double Kr[M][L] = {{ 0, 5.774, 0, 4.309, 0.408}, 00138 {-5, 2.887,-3.732, 2.155,-0.408}, 00139 {-5,-2.887,-3.732,-2.155, 0.408}, 00140 { 0,-5.774, 0,-4.309,-0.408}, 00141 { 5,-2.887, 3.732,-2.155, 0.408}, 00142 { 5, 2.887, 3.732, 2.155,-0.408}}; 00143 00144 // Gain (q1=100, q2=100, q3=100, q4=100, q5=1) 00145 /* double Kr[M][L] = {{ 0, 5.774, 0, 5.936, 0.408}, 00146 {-5, 2.887,-5.141, 2.968,-0.408}, 00147 {-5,-2.887,-5.141,-2.968, 0.408}, 00148 { 0,-5.774, 0,-5.936,-0.408}, 00149 { 5,-2.887, 5.141,-2.968, 0.408}, 00150 { 5, 2.887, 5.141, 2.968,-0.408}}; 00151 */ 00152 // Gain (q1=300, q2=300, q3=100, q4=100, q5=1) 00153 /* double Kr[M][L] = {{ 0, 10, 0, 6.052, 0.408}, 00154 {-8.66, 5,-5.242, 3.026,-0.408}, 00155 {-8.66, -5,-5.242,-3.026, 0.408}, 00156 { 0,-10, 0,-6.052,-0.408}, 00157 { 8.66, -5, 5.242,-3.026, 0.408}, 00158 { 8.66, 5, 5.242, 3.026,-0.408}}; 00159 */ 00160 // Gain (q1=400, q2=400, q3=100, q4=100, q5=1) 00161 /* double Kr[M][L] = {{ 0, 11.547, 0, 6.094, 0.408}, 00162 {-10, 5.774,-5.278, 3.047,-0.408}, 00163 {-10, -5.774,-5.278,-3.047, 0.408}, 00164 { 0,-11.547, 0,-6.094,-0.408}, 00165 { 10, -5.774, 5.278,-3.047, 0.408}, 00166 { 10, 5.774, 5.278, 3.047,-0.408}}; 00167 */ 00168 // Gain (q1=500, q2=500, q3=50, q4=50, q5=1) 00169 /* double Kr[M][L] = {{ 0, 12.910, 0, 4.574, 0.408}, 00170 {-11.18, 6.455,-3.962, 2.287,-0.408}, 00171 {-11.18, -6.455,-3.962,-2.287, 0.408}, 00172 { 0,-12.910, 0,-4.574,-0.408}, 00173 { 11.18, -6.455, 3.962,-2.287, 0.408}, 00174 { 11.18, 6.455, 3.962, 2.287,-0.408}}; 00175 */ 00176 // Gain (q1=500, q2=500, q3=100, q4=100, q5=1) 00177 /* double Kr[M][L] = {{ 0, 12.910, 0, 6.131, 0.408}, 00178 {-11.18, 6.455,-5.31, 3.066,-0.408}, 00179 {-11.18, -6.455,-5.31,-3.066, 0.408}, 00180 { 0,-12.910, 0,-6.131,-0.408}, 00181 { 11.18, -6.455, 5.31,-3.066, 0.408}, 00182 { 11.18, 6.455, 5.31, 3.066,-0.408}}; 00183 */ 00184 double Ccom_r = 0.2; 00185 double Ccom_p = 0.2; 00186 double Ccom_y = 0.1; 00187 00188 double motor[M] = {0}; 00189 double motor_control[M] = {0}; // Pulth width generated by the transmitter 00190 double motor_attitude[M] = {0}; // Pulth width generated by the attitude control 00191 00192 // Coefficient of transfering Force to Puls 00193 double Cfp = 2*pow(10.0, -7); 00194 00195 // Navigation 00196 char command_buf1[chan_buf] = {0}; 00197 int command_buf2[chan_buf] = {0}; 00198 double command[chan] = {0}; 00199 int set_navi_flag = 0; 00200 00201 // *** Setting up sensors *** 00202 //pc.printf("\r\n\r\nSetting up sensors\r\n"); 00203 xbee.printf("\r\n\r\nSetting up sensors\r\n"); 00204 00205 // These are here to test whether any of the initialization fails. It will print the failure. 00206 if (accelerometer.setPowerControl(0x00)){ 00207 //pc.printf("didn't intitialize power control\n\r"); 00208 xbee.printf("didn't intitialize power control\n\r"); 00209 return 0; 00210 } 00211 // Full resolution, +/-16g, 4mg/LSB. 00212 wait(.1); 00213 00214 if(accelerometer.setDataFormatControl(0x0B)){ 00215 //pc.printf("didn't set data format\n\r"); 00216 xbee.printf("didn't set data format\n\r"); 00217 return 0; 00218 } 00219 wait(.1); 00220 00221 // 3.2kHz data rate. 00222 if(accelerometer.setDataRate(ADXL345_3200HZ)){ 00223 //pc.printf("didn't set data rate\n\r"); 00224 xbee.printf("didn't set data rate\n\r"); 00225 return 0; 00226 } 00227 wait(.1); 00228 00229 if(accelerometer.setPowerControl(MeasurementMode)) { 00230 //pc.printf("didn't set the power control to measurement\n\r"); 00231 xbee.printf("didn't set the power control to measurement\n\r"); 00232 return 0; 00233 } 00234 wait(.1); 00235 00236 gyro.setLpBandwidth(LPFBW_42HZ); 00237 compass.setDefault(); 00238 wait(.1); // Wait some time for all sensors (Need at least 5ms) 00239 00240 // *** Setting up motors *** 00241 //pc.printf("Setting up motors\r\n"); 00242 xbee.printf("Setting up motors\r\n"); 00243 ESC1.period(0.016044); ESC2.period(0.016044); ESC3.period(0.016044); ESC4.period(0.016044); ESC5.period(0.016044); ESC6.period(0.016044); 00244 // pulsewidth 0.0011~0.00195 00245 ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); 00246 // Wait some time for ESC (about 10s) 00247 wait(5); 00248 00249 // *** Setting up navigator *** 00250 //pc.printf("Setting up navigator\r\n"); 00251 xbee.printf("Setting up navigator\r\n"); 00252 while( set_navi_flag==0 ){ 00253 navi_flag = 1; 00254 for( int i=0;i<chan_buf;i++ ){ 00255 navi.scanf("%c",&command_buf1[i]); 00256 if(command_buf1[i]=='a'){ 00257 set_navi_flag=1; 00258 break; 00259 } 00260 } 00261 navi_flag = 0; 00262 wait(.1); 00263 } 00264 00265 // *** Calibration routine *** 00266 p_calib = calib(); 00267 for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; } 00268 for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; } 00269 //compass_basis_rad = *p_calib; 00270 00271 led1 = 1; led2 = 1; led3 = 1; led4 = 1; 00272 00273 //pc.printf("Starting IMU unit\n\r"); 00274 xbee.printf("Starting IMU unit\n\r"); 00275 //pc.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r"); 00276 xbee.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r"); 00277 00278 //while(1){ 00279 while( stop==0 ){ 00280 00281 // Navigation 00282 if( navi_loop>=navi_period ){ 00283 00284 navi_flag = 1; 00285 00286 for( int i=0;i<chan_buf;i++ ){ navi.scanf("%c",&command_buf1[i]); } 00287 for( int i=0;i<chan_buf;i++ ){ 00288 command_buf2[i] = (int)command_buf1[i]-48; 00289 if( command_buf2[i]==-16 ){ command_buf2[i]=0; } 00290 } 00291 //for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; } 00292 command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001; 00293 for( int i=1;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]-0.0005; } 00294 00295 navi_flag = 0; 00296 navi_loop = 0; 00297 00298 } 00299 navi_loop++; 00300 //command[0] = 0.0016; // Shiyougo comentout surukoto!! 00301 00302 // Updating accelerometer and compass 00303 accelerometer.getOutput(bit_acc); 00304 compass.readData(get_mag); 00305 00306 for( int i=0;i<N;i++ ){ acc0[i] = acc[i]; } 00307 00308 // Transfering units and Coordinate transform 00309 acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); 00310 acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); 00311 acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1; 00312 00313 gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); 00314 gyro_deg[1] = (-(gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); // Modify the differencial of the sensor axis 00315 gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375; 00316 /* 00317 for( int i=0;i<N;i++ ){ acc[i] = (int16_t)bit_acc[i]; } 00318 gyro_deg[0] = gyro.getGyroX(); 00319 gyro_deg[1] = gyro.getGyroY(); 00320 gyro_deg[2] = gyro.getGyroZ(); 00321 */ 00322 //for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; } 00323 00324 // Low pass filter for acc 00325 //if( -0.05<acc[0] && acc[0]<0.05 ){ acc[0]=0; } 00326 //if( -0.05<acc[1] && acc[1]<0.05 ){ acc[1]=0; } 00327 //if( 0.95<acc[2] && acc[2]<1.05 ){ acc[2]=0; } 00328 // Limitter for acc 00329 if( acc[0]<-1 ){ acc[0]=-1; } if( 1<acc[0] ){ acc[0]=1; } 00330 if( acc[1]<-1 ){ acc[1]=-1; } if( 1<acc[1] ){ acc[1]=1; } 00331 if( acc[2]<-0 ){ acc[2]=-0; } if( 2<acc[2] ){ acc[2]=2; } 00332 00333 for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc0[i]; } 00334 if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc0[0]; } 00335 if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc0[1]; } 00336 if( abs(d_acc[2])>=0.5 ){ acc[2] = acc0[2]; } 00337 00338 // Low pass filter for gyro 00339 //for( int i=0;i<N;i++ ){if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; }} 00340 // Limitter for gyro 00341 //for( int i=0;i<N;i++ ){if( gyro_deg[i]<-90 ){ gyro_deg[i]=-90; }} for( int i=0;i<N;i++ ){if( 90<gyro_deg[i] ){ gyro_deg[i]=90; }} 00342 00343 for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; } 00344 /* 00345 // Compass yaw 00346 compass_rad = (double)mag[1]/mag[0]; 00347 compass_rad = atan(compass_rad); 00348 //compass_rad = compass_rad-compass_basis_rad; 00349 compass_deg = compass_rad*180/pi; 00350 */ 00351 // LWMA (Observation) 00352 p_zLWMA = LWMA(acc); 00353 for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; } 00354 // LWMA angle 00355 //angle_acc[0] = asin(zLWMA[1])*180/pi; 00356 //angle_acc[1] = asin(zLWMA[0])*180/pi; 00357 00358 // RK4 (Prediction) 00359 p_RK4 = RK4(dt,angle_rad,gyro_rad); 00360 for( int i=0;i<N;i++ ){ angle_rad[i] = *p_RK4; p_RK4 = p_RK4+1; } 00361 00362 // EKF (Correction) 00363 EKF_predict(angle_rad,gyro_rad); 00364 if ( correct_loop>=correct_period ){ 00365 p_EKF = EKF_correct(angle_rad,gyro_rad,zLWMA); 00366 for ( int i=0;i<N;i++ ){ angle_rad[i] = *p_EKF; p_EKF = p_EKF+1; } 00367 correct_loop = 0; 00368 } 00369 correct_loop++; 00370 00371 for( int i=0;i<N;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; } 00372 00373 state[0] = angle_deg[0]; state[1] = angle_deg[1]; 00374 state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2]; 00375 00376 for( int i=0;i<M;i++ ){ 00377 motor_attitude[i] = 0; 00378 for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; } 00379 } 00380 00381 motor_control[0] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]+1*Ccom_y*command[3]; 00382 motor_control[1] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; 00383 motor_control[2] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; 00384 motor_control[3] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]-1*Ccom_y*command[3]; 00385 motor_control[4] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; 00386 motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; 00387 00388 for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; } 00389 //for( int i=0;i<M;i++ ){ motor[i] = command[0]; } 00390 00391 // pulsewidth 0.0011~0.0019 (0.001~0.002?) 00392 for( int i=0;i<M;i++ ){ 00393 if( motor[i]>0.00195 ){ motor[i]=0.00195; } 00394 if( motor[i]<0.0011 ){ motor[i]=0.00105; } 00395 if( command[0]<0.0011 ){ motor[i]=0.00105; } 00396 } 00397 00398 ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]); 00399 00400 //for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; } 00401 00402 //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]); 00403 //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); 00404 //fprintf(fp,"%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %6.3f\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], command[0]*1000); 00405 fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); 00406 if( xbee_loop>=xbee_period ){ 00407 xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); 00408 xbee_loop = 0; 00409 } 00410 xbee_loop++; 00411 wait(dt_wait); 00412 t += dt; 00413 00414 } 00415 00416 // pulsewidth 0.0011~0.00195 00417 ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); 00418 led1 = 0; led2 = 0; led3 = 0; led4 = 0; 00419 fclose(fp); 00420 00421 } 00422 00423 double* EKF_predict( double y[N], double x[N] ){ 00424 // x = F * x; 00425 // P = F * P * F' + G * Q * G'; 00426 00427 //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} }; 00428 double Q[N][N] = { {5198, 0, 0}, {0, 5518, 0}, {0, 0, 5722} }; 00429 00430 double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}}; 00431 double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}}; 00432 double Gjac[N][N] = {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}}; 00433 double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}}; 00434 00435 double FPF[N][N] = {0}; 00436 double GQG[N][N] = {0}; 00437 00438 double FP[N][N] = {0}; 00439 double GQ[N][N] = {0}; 00440 00441 for( int i=0;i<N;i++ ){ 00442 for( int j=0;j<N;j++ ){ 00443 for( int k=0;k<N;k++ ){ 00444 FP[i][j] += Fjac[i][k]*P[k][j]; 00445 GQ[i][j] += Gjac[i][k]*Q[k][j]; 00446 00447 } 00448 } 00449 } 00450 00451 for( int i=0;i<N;i++ ){ 00452 for( int j=0;j<N;j++ ){ 00453 for( int k=0;k<N;k++ ){ 00454 FPF[i][j] += FP[i][k]*Fjac_t[k][j]; 00455 GQG[i][j] += GQ[i][k]*Gjac_t[k][j]; 00456 } 00457 } 00458 } 00459 for( int i=0;i<N;i++ ){ 00460 for( int j=0;j<N;j++ ){ 00461 P[i][j] = FPF[i][j]+GQG[i][j]; 00462 } 00463 } 00464 00465 return 0; 00466 00467 } 00468 00469 double* EKF_correct( double y[N], double x[N], double z[N] ){ 00470 // K = P * H' / ( H * P * H' + R ) 00471 // x = x + K * ( yobs(t) - H * x ) 00472 // P = P - K * H * P 00473 00474 //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} }; 00475 //double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} }; 00476 double R[N][N] = { {272528, 0, 0}, {0, 295812, 0}, {0, 0, 908451} }; 00477 00478 double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}}; 00479 double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}}; 00480 double K[N][N] = {0}; // Kalman gain 00481 double K_deno[N][N] = {0}; // Denominator of the kalman gain 00482 double det_K_deno_inv = 0; 00483 double K_deno_inv[N][N] = {0}; 00484 double HPH[N][N] = {0}; 00485 double HP[N][N] = {0}; 00486 double PH[N][N] = {0}; 00487 double KHP[N][N] = {0}; 00488 00489 double Hx[N] = {0}; 00490 double z_Hx[N] = {0}; 00491 double Kz_Hx[N] = {0}; 00492 00493 double* py = y; 00494 00495 // Kalman gain 00496 for( int i=0;i<N;i++ ){ 00497 for( int j=0;j<N;j++ ){ 00498 for( int k=0;k<N;k++ ){ 00499 HP[i][j] += Hjac[i][k]*P[k][j]; 00500 PH[i][j] += P[i][k]*Hjac_t[k][j]; 00501 } 00502 } 00503 } 00504 for( int i=0;i<N;i++ ){ 00505 for( int j=0;j<N;j++ ){ 00506 for( int k=0;k<N;k++ ){ 00507 HPH[i][j] += HP[i][k]*Hjac_t[k][j]; 00508 } 00509 } 00510 } 00511 for( int i=0;i<N;i++ ){ 00512 for( int j=0;j<N;j++ ){ 00513 K_deno[i][j] = HPH[i][j]+R[i][j]; 00514 } 00515 } 00516 // Inverce matrix 00517 det_K_deno_inv = K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2]; 00518 K_deno_inv[0][0] = (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv; 00519 K_deno_inv[0][1] = (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv; 00520 K_deno_inv[0][2] = (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv; 00521 K_deno_inv[1][0] = (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv; 00522 K_deno_inv[1][1] = (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv; 00523 K_deno_inv[1][2] = (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv; 00524 K_deno_inv[2][0] = (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv; 00525 K_deno_inv[2][1] = (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv; 00526 K_deno_inv[2][2] = (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv; 00527 00528 for( int i=0;i<N;i++ ){ 00529 for( int j=0;j<N;j++ ){ 00530 for( int k=0;k<N;k++ ){ 00531 K[i][j] += PH[i][k]*K_deno_inv[k][j]; 00532 } 00533 } 00534 } 00535 00536 // Filtering 00537 for( int i=0;i<N;i++ ){ 00538 for( int j=0;j<N;j++ ){ 00539 Hx[i] += Hjac[i][j]*y[j]; 00540 } 00541 } 00542 for( int i=0;i<N;i++ ){ 00543 z_Hx[i] = z[i]-Hx[i]; 00544 } 00545 for( int i=0;i<N;i++ ){ 00546 for( int j=0;j<N;j++ ){ 00547 Kz_Hx[i] += K[i][j]*z_Hx[j]; 00548 } 00549 } 00550 for( int i=0;i<N;i++ ){ 00551 y[i] = y[i]+Kz_Hx[i]; 00552 } 00553 00554 // Covarience 00555 for( int i=0;i<N;i++ ){ 00556 for( int j=0;j<N;j++ ){ 00557 for( int k=0;k<N;k++ ){ 00558 KHP[i][j] += K[i][k]*HP[k][j]; 00559 } 00560 } 00561 } 00562 for( int i=0;i<N;i++ ){ 00563 for( int j=0;j<N;j++ ){ 00564 P[i][j] = P[i][j]-KHP[i][j]; 00565 } 00566 } 00567 00568 return py; 00569 00570 } 00571 00572 double* LWMA( double z[N] ){ 00573 00574 double zLWMA[N] = {0}; 00575 double zLWMA_num[N] = {0}; 00576 double zLWMA_den = 0; 00577 00578 double* pzLWMA = zLWMA; 00579 00580 for( int i=1;i<N_LWMA;i++ ){ 00581 for( int j=0;j<N;j++ ){ 00582 z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1]; 00583 } 00584 } 00585 for( int i=0;i<N;i++ ){ 00586 z_buf[i][0] = z[i]; 00587 } 00588 00589 for( int i=0;i<N_LWMA;i++ ){ 00590 for( int j=0;j<N;j++ ){ 00591 zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i]; 00592 } 00593 zLWMA_den += N_LWMA-i; 00594 } 00595 for( int i=0;i<N;i++ ){ 00596 zLWMA[i] = zLWMA_num[i]/zLWMA_den; 00597 } 00598 00599 return pzLWMA; 00600 00601 } 00602 00603 double* RK4( double dt, double y[N], double x[N] ){ 00604 00605 double yBuf[N] = {0}; 00606 double k[N][4] = {0}; 00607 00608 double* p_y = y; 00609 00610 double* pk1; 00611 double* pk2; 00612 double* pk3; 00613 double* pk4; 00614 00615 for( int i=0;i<N;i++){ yBuf[i] = y[i]; } 00616 pk1 = func (yBuf,x); 00617 for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } 00618 00619 for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } 00620 pk2 = func (yBuf,x); 00621 for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } 00622 00623 for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } 00624 pk3 = func (yBuf,x); 00625 for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } 00626 00627 for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; } 00628 pk4 = func (yBuf,x); 00629 for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } 00630 00631 for( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; } 00632 00633 return p_y; 00634 00635 } 00636 00637 double* func( double y[N], double x[N] ){ 00638 00639 double f[N] = {0}; 00640 double* p_f = f; 00641 00642 f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]); 00643 f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]); 00644 f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]); 00645 00646 return p_f; 00647 00648 } 00649 00650 double* calib(){ 00651 00652 int calib_loop = 1000; 00653 00654 int bit_acc[N] = {0}; // Buffer of the accelerometer 00655 int get_mag[N] = {0}; // Buffer of the compass 00656 00657 double calib_acc[N] = {0}; 00658 double calib_gyro_buf[N] = {0}; 00659 double calib_gyro[N] = {0}; 00660 double compass_basis_buf[N] = {0}; 00661 double compass_basis_rad = 0; 00662 double calib_result[7] = {0}; 00663 00664 double* p_calib_result = calib_result; 00665 00666 pc.printf("Don't touch... Calibrating now!!\n\r"); 00667 xbee.printf("Don't touch... Calibrating now!!\n\r"); 00668 led1 = 1; 00669 00670 for( int i=0;i<calib_loop;i++ ){ 00671 00672 accelerometer.getOutput(bit_acc); 00673 compass.readData(get_mag); 00674 00675 calib_gyro_buf[0] = gyro.getGyroX(); 00676 calib_gyro_buf[1] = gyro.getGyroY(); 00677 calib_gyro_buf[2] = gyro.getGyroZ(); 00678 00679 for( int j=0;j<N;j++ ){ 00680 calib_acc[j] += (int16_t)bit_acc[j]; 00681 calib_gyro[j] += calib_gyro_buf[j]; 00682 compass_basis_buf[j] += (int16_t)get_mag[j]; 00683 } 00684 00685 if( i>calib_loop*3/4 ){ 00686 led4 = 1; 00687 }else if( i>calib_loop/2 ){ 00688 led3 = 1; 00689 }else if( i>calib_loop/4 ){ 00690 led2 = 1; 00691 } 00692 00693 } 00694 00695 for( int i=0;i<N;i++ ){ 00696 calib_acc[i] = calib_acc[i]/calib_loop; 00697 calib_gyro[i] = calib_gyro[i]/calib_loop; 00698 compass_basis_buf[i] = compass_basis_buf[i]/calib_loop; 00699 } 00700 00701 compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0]; 00702 compass_basis_rad = atan(compass_basis_rad); 00703 led1 = 0; led2 = 0; led3 = 0; led4 = 0; 00704 00705 pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); 00706 xbee.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); 00707 pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi); 00708 xbee.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi); 00709 00710 for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; } 00711 for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; } 00712 calib_result[6] = compass_basis_rad; 00713 00714 if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){ 00715 pc.printf("Accelerometer is not available.\r\n"); 00716 xbee.printf("Accelerometer is not available.\r\n"); 00717 } 00718 00719 for( int i=0;i<3;i++ ){ 00720 wait(.5); 00721 led1 = 1; led2 = 1; led3 = 1; led4 = 1; 00722 wait(.5); 00723 led1 = 0; led2 = 0; led3 = 0; led4 = 0; 00724 } 00725 00726 return p_calib_result; 00727 00728 }
Generated on Mon Jul 18 2022 10:05:39 by
1.7.2