Dependencies: mbed FatFileSystem
Diff: main.cpp
- Revision:
- 0:75227c386257
- Child:
- 1:0c42db451cb9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Jun 10 08:44:32 2012 +0000 @@ -0,0 +1,654 @@ +// dot-HR EKF + +#include "mbed.h" +#include "SDFileSystem.h" +#include "ADXL345_I2C.h" +#include "ITG3200.h" +#include "HMC5883L.h" + +Serial pc(USBTX, USBRX); +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); +SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board +ADXL345_I2C accelerometer(p9, p10); +ITG3200 gyro(p9, p10); +HMC5883L compass(p9, p10); +Serial xbee(p13, p14); +DigitalIn stop(p20); +PwmOut ESC1(p21); +PwmOut ESC2(p22); +PwmOut ESC3(p23); +PwmOut ESC4(p24); +PwmOut ESC5(p25); +PwmOut ESC6(p26); +Serial navi(p28, p27); // tx, rx +DigitalOut navi_flag(p29); + +#define pi 3.14159265 + +#define N 3 // gyro, acc +#define N_2 2 // phi, the +#define M 6 // The numbers of rotors +#define L 5 // The numbers of state quantities (optimal regulator) +#define chan 4 // The numbers of channels +#define chan_buf 17 +#define N_LWMA 100 + +double* calib(); +double* RK4( double, double[N_2], double[N_2] ); +double* func( double[N_2], double[N_2] ); +double* LWMA( double[N] ); +double* EKF_predict( double[N_2], double[N_2] ); +double* EKF_correct( double[N_2], double[N_2], double[N_2] ); + +// 0 1 2 +// [ accXn-1 accXn-2 ... ] 0 +// zBuf = [ accYn-1 accYn-2 ... ] 1 +// [ accZn-1 accZn-2 ... ] 2 +double z_buf[N][N_LWMA] = {0}; // For LWMA + +double P[N_2][N_2] = {{1,0},{0,1}}; // For EKF + +int main(){ + + pc.baud(921600); + navi.baud(57600); + xbee.baud(57600); + + FILE *fp = fopen("/sd/sdtest.txt", "w"); + if(fp == NULL) { + error("Could not open file for write\n"); + while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; } + } + + int correct_period = 100; + int navi_period = 10; + int xbee_period = 100; + int correct_loop = 0; + int navi_loop = 10; + int xbee_loop = 100; + + double dt_wait = 0.002; + //double dt_wait = 0.01; + double dt = 0.01; + double t = 0; + + int bit_acc[N] = {0}; // Buffer of the accelerometer + int get_mag[N] = {0}; // Buffer of the compass + + // Calibration routine + double calib_acc[N] = {0}; + double calib_gyro[N] = {0}; + //double compass_basis_rad = 0; + + // Getting data + double acc[N] = {0, 0, 1}; + double acc_pre[N] = {0, 0, 1}; + double d_acc[N] = {0}; + double gyro_deg[N] = {0}; + //double gyro_deg_pre[N] = {0}; + double gyro_rad[N_2] = {0}; + + // Measurement algorithm + double angle_deg[N_2] = {0}; + double angle_rad[N_2] = {0}; + double angle_rad_pre[N_2] = {0}; + double zLWMA[N] = {0}; + double zLWMA_2[N_2] = {0}; + + // Gravity z + for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; } + + double* p_calib; + double* p_RK4; + double* p_EKF; + double* p_zLWMA; + + // Optimal regulator + double state[L]; + + + // Gain (q1=100, q2=100, q3=50, q4=50, q5=1) + + // Gain (q1=100, q2=100, q3=50, q4=50, q5=5) + + // Gain (q1=100, q2=100, q3=50, q4=50, q5=10) + + // Gain (q1=100, q2=100, q3=50, q4=50, q5=20) + + // Gain (q1=100, q2=100, q3=50, q4=50, q5=30) + double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.408}, + {-5, 2.887,-3.610, 2.084,-0.408}, + {-5,-2.887,-3.610,-2.084,-0.408}, + { 0,-5.774, 0,-4.168, 0.408}, + { 5,-2.887, 3.610,-2.084,-0.408}, + { 5, 2.887, 3.610, 2.084, 0.408}}; + + // Coefficient for moving control + double Ccom_r = 0.2; + double Ccom_p = 0.2; + double Ccom_y = 0.1; + + double motor[M] = {0}; + double motor_control[M] = {0}; // Pulth width generated by the transmitter + double motor_attitude[M] = {0}; // Pulth width generated by the attitude control + + // Coefficient of transfering Force to Puls + double Cfp = 2*pow(10.0, -7); + + // Navigation + char command_buf1[chan_buf] = {0}; + int command_buf2[chan_buf] = {0}; + double command[chan] = {0}; + int set_navi_flag = 0; + + // *** Setting up sensors *** + //pc.printf("\r\n\r\nSetting up sensors\r\n"); + xbee.printf("\r\n\r\nSetting up sensors\r\n"); + + // These are here to test whether any of the initialization fails. It will print the failure. + if (accelerometer.setPowerControl(0x00)){ + //pc.printf("didn't intitialize power control\n\r"); + xbee.printf("didn't intitialize power control\n\r"); + return 0; + } + // Full resolution, +/-16g, 4mg/LSB. + wait(.1); + + if(accelerometer.setDataFormatControl(0x0B)){ + //pc.printf("didn't set data format\n\r"); + xbee.printf("didn't set data format\n\r"); + return 0; + } + wait(.1); + + // 3.2kHz data rate. + if(accelerometer.setDataRate(ADXL345_3200HZ)){ + //pc.printf("didn't set data rate\n\r"); + xbee.printf("didn't set data rate\n\r"); + return 0; + } + wait(.1); + + if(accelerometer.setPowerControl(MeasurementMode)) { + //pc.printf("didn't set the power control to measurement\n\r"); + xbee.printf("didn't set the power control to measurement\n\r"); + return 0; + } + wait(.1); + + gyro.setLpBandwidth(LPFBW_42HZ); + compass.setDefault(); + wait(.1); // Wait some time for all sensors (Need at least 5ms) + + // *** Setting up motors *** + //pc.printf("Setting up motors\r\n"); + xbee.printf("Setting up motors\r\n"); + ESC1.period(0.016044); ESC2.period(0.016044); ESC3.period(0.016044); ESC4.period(0.016044); ESC5.period(0.016044); ESC6.period(0.016044); + // pulsewidth 0.0011~0.00195 + ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); + // Wait some time for ESC (about 10s) + wait(5); + + // *** Setting up navigator *** + //pc.printf("Setting up navigator\r\n"); + xbee.printf("Setting up navigator\r\n"); + while( set_navi_flag==0 ){ + navi_flag = 1; + for( int i=0;i<chan_buf;i++ ){ + navi.scanf("%c",&command_buf1[i]); + if(command_buf1[i]=='a'){ + set_navi_flag=1; + break; + } + } + navi_flag = 0; + wait(.1); + } + + // *** Calibration routine *** + p_calib = calib(); + for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; } + for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; } + //compass_basis_rad = *p_calib; + + led1 = 1; led2 = 1; led3 = 1; led4 = 1; + + //pc.printf("Starting IMU unit\n\r"); + xbee.printf("Starting IMU unit\n\r"); + //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"); + 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"); + + //while(1){ + while( stop==0 ){ + + // Navigation + if( navi_loop>=navi_period ){ + + navi_flag = 1; + + for( int i=0;i<chan_buf;i++ ){ navi.scanf("%c",&command_buf1[i]); } + for( int i=0;i<chan_buf;i++ ){ + command_buf2[i] = (int)command_buf1[i]-48; + if( command_buf2[i]==-16 ){ command_buf2[i]=0; } + } + //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; } + command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001; + 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; } + + navi_flag = 0; + navi_loop = 0; + + } + navi_loop++; + //command[0] = 0.0016; // Shiyougo comentout surukoto!! + + // Updating accelerometer and compass + accelerometer.getOutput(bit_acc); + compass.readData(get_mag); + + // Transfering units and Coordinate transform + acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); + acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); + acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1; + + gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); + 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 + gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375; + + // Limitter for acc + if( acc[0]<-1 ){ acc[0] = -1; } if( 1<acc[0] ){ acc[0] = 1; } + if( acc[1]<-1 ){ acc[1] = -1; } if( 1<acc[1] ){ acc[1] = 1; } + if( acc[2]<-0 ){ acc[2] = -0; } if( 2<acc[2] ){ acc[2] = 2; } + + for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc_pre[i]; } + if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc_pre[0]; } + if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc_pre[1]; } + if( abs(d_acc[2])>=0.5 ){ acc[2] = acc_pre[2]; } + + for( int i=0;i<N_2;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; } + + // LWMA (Observation) + p_zLWMA = LWMA(acc); + for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; } + + for( int i=0;i<N_2;i++ ){ zLWMA_2[i] = zLWMA[i]; } + + // RK4 (Prediction) + p_RK4 = RK4(dt,angle_rad,gyro_rad); + for( int i=0;i<N_2;i++ ){ angle_rad[i] = *p_RK4; p_RK4 = p_RK4+1; } + + // EKF (Correction) + EKF_predict(angle_rad,gyro_rad); + if ( correct_loop>=correct_period ){ + p_EKF = EKF_correct(angle_rad,gyro_rad,zLWMA_2); + for ( int i=0;i<N_2;i++ ){ angle_rad[i] = *p_EKF; p_EKF = p_EKF+1; } + correct_loop = 0; + } + correct_loop++; + + // Limitter for angle + for( int i=0;i<N_2;i++ ){ if( abs(angle_rad[i])>=1.05 ){ angle_rad[i] = angle_rad_pre[i]; } } + + for( int i=0;i<N_2;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; } + + state[0] = angle_deg[0]; state[1] = angle_deg[1]; + state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2]; + + for( int i=0;i<M;i++ ){ + motor_attitude[i] = 0; + for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; } + } + + motor_control[0] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]+1*Ccom_y*command[3]; + motor_control[1] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; + motor_control[2] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; + motor_control[3] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]-1*Ccom_y*command[3]; + motor_control[4] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; + motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; + + for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; } + //for( int i=0;i<M;i++ ){ motor[i] = command[0]; } + + // pulsewidth 0.0011~0.0019 (0.001~0.002?) + for( int i=0;i<M;i++ ){ + if( motor[i]>0.00195 ){ motor[i]=0.00195; } + if( motor[i]<0.0012 ){ motor[i]=0.00105; } + if( command[0]<0.0011 ){ motor[i]=0.00105; } + } + + ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]); + + //for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; } + + //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]); + //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %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], 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); + //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); + 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); + if( xbee_loop>=xbee_period ){ + 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); + xbee_loop = 0; + } + + for( int i=0;i<N;i++ ){ acc_pre[i] = acc[i]; } + //for( int i=0;i<N;i++ ){ gyro_deg_pre[i] = gyro_deg[i]; } + for( int i=0;i<N_2;i++ ){ angle_rad_pre[i] = angle_rad[i]; } + + xbee_loop++; + wait(dt_wait); + t += dt; + + } + + // pulsewidth 0.0011~0.00195 + ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); + led1 = 0; led2 = 0; led3 = 0; led4 = 0; + fclose(fp); + +} + +double* EKF_predict( double y[N_2], double x[N_2] ){ + // x = F * x; + // P = F * P * F' + G * Q * G'; + + //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} }; + double Q[N_2][N_2] = { {5198, 0}, {0, 5518} }; + + double Fjac[N_2][N_2] = { {x[1]*cos(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))}, {-x[1]*sin(y[0]), 0} }; + double Fjac_t[N_2][N_2] = { {x[1]*cos(y[0])*tan(y[1]), -x[1]*sin(y[0])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1])), 0} }; + double Gjac[N_2][N_2] = { {1, sin(y[0])*tan(y[1])}, {0, cos(y[0])} }; + double Gjac_t[N_2][N_2] = { {1, 0}, {sin(y[0])*tan(y[1]), cos(y[0])} }; + + double FPF[N_2][N_2] = {0}; + double GQG[N_2][N_2] = {0}; + + double FP[N_2][N_2] = {0}; + double GQ[N_2][N_2] = {0}; + + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + FP[i][j] += Fjac[i][k]*P[k][j]; + GQ[i][j] += Gjac[i][k]*Q[k][j]; + + } + } + } + + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + FPF[i][j] += FP[i][k]*Fjac_t[k][j]; + GQG[i][j] += GQ[i][k]*Gjac_t[k][j]; + } + } + } + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + P[i][j] = FPF[i][j]+GQG[i][j]; + } + } + + return 0; + +} + +double* EKF_correct( double y[N_2], double x[N_2], double z[N_2] ){ + // K = P * H' / ( H * P * H' + R ) + // x = x + K * ( yobs(t) - H * x ) + // P = P - K * H * P + + //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} }; + //double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} }; + double R[N_2][N_2] = { {272528, 0}, {0, 295812} }; + + double Hjac[N_2][N_2] = { {0, cos(y[1])}, {cos(y[0]), 0} }; + double Hjac_t[N_2][N_2] = { {0, cos(y[0])}, {cos(y[1]), 0} }; + double K[N_2][N_2] = {0}; // Kalman gain + double K_deno[N_2][N_2] = {0}; // Denominator of the kalman gain + double det_K_deno_inv = 0; + double K_deno_inv[N_2][N_2] = {0}; + double HPH[N_2][N_2] = {0}; + double HP[N_2][N_2] = {0}; + double PH[N_2][N_2] = {0}; + double KHP[N_2][N_2] = {0}; + + double Hx[N_2] = {0}; + double z_Hx[N_2] = {0}; + double Kz_Hx[N_2] = {0}; + + double* py = y; + + // Kalman gain + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + HP[i][j] += Hjac[i][k]*P[k][j]; + PH[i][j] += P[i][k]*Hjac_t[k][j]; + } + } + } + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + HPH[i][j] += HP[i][k]*Hjac_t[k][j]; + } + } + } + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + K_deno[i][j] = HPH[i][j]+R[i][j]; + } + } + // Inverce matrix + det_K_deno_inv = K_deno[0][0]*K_deno[1][1]-K_deno[1][0]*K_deno[0][1]; + K_deno_inv[0][0] = K_deno[1][1]/det_K_deno_inv; + K_deno_inv[0][1] = -K_deno[0][1]/det_K_deno_inv; + K_deno_inv[1][0] = -K_deno[1][0]/det_K_deno_inv; + K_deno_inv[1][1] = K_deno[0][0]/det_K_deno_inv; + + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + K[i][j] += PH[i][k]*K_deno_inv[k][j]; + } + } + } + + // Filtering + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + Hx[i] += Hjac[i][j]*y[j]; + } + } + for( int i=0;i<N_2;i++ ){ + z_Hx[i] = z[i]-Hx[i]; + } + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + Kz_Hx[i] += K[i][j]*z_Hx[j]; + } + } + for( int i=0;i<N_2;i++ ){ + y[i] = y[i]+Kz_Hx[i]; + } + + // Covarience + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + for( int k=0;k<N_2;k++ ){ + KHP[i][j] += K[i][k]*HP[k][j]; + } + } + } + for( int i=0;i<N_2;i++ ){ + for( int j=0;j<N_2;j++ ){ + P[i][j] = P[i][j]-KHP[i][j]; + } + } + + return py; + +} + +double* LWMA( double z[N] ){ + + double zLWMA[N] = {0}; + double zLWMA_num[N] = {0}; + double zLWMA_den = 0; + + double* pzLWMA = zLWMA; + + for( int i=1;i<N_LWMA;i++ ){ + for( int j=0;j<N;j++ ){ + z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1]; + } + } + for( int i=0;i<N;i++ ){ + z_buf[i][0] = z[i]; + } + + for( int i=0;i<N_LWMA;i++ ){ + for( int j=0;j<N;j++ ){ + zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i]; + } + zLWMA_den += N_LWMA-i; + } + for( int i=0;i<N;i++ ){ + zLWMA[i] = zLWMA_num[i]/zLWMA_den; + } + + return pzLWMA; + +} + +double* RK4( double dt, double y[N_2], double x[N_2] ){ + + double yBuf[N_2] = {0}; + double k[N_2][4] = {0}; + + double* p_y = y; + + double* pk1; + double* pk2; + double* pk3; + double* pk4; + + for( int i=0;i<N_2;i++){ yBuf[i] = y[i]; } + pk1 = func (yBuf,x); + for( int i=0;i<N_2;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } + + for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } + pk2 = func (yBuf,x); + for( int i=0;i<N_2;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } + + for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } + pk3 = func (yBuf,x); + for( int i=0;i<N_2;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } + + for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+dt*k[i][3]; } + pk4 = func (yBuf,x); + for( int i=0;i<N_2;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } + + for( int i=0;i<N_2;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; } + + return p_y; + +} + +double* func( double y[N_2], double x[N_2] ){ + + double f[N_2] = {0}; + double* p_f = f; + double a; + double b; + + f[0] = x[0]+x[1]*sin(y[0])*tan(y[1]); + f[1] = x[1]*cos(y[0]); + a = sin(b); + + return p_f; + +} + +double* calib(){ + + int calib_loop = 1000; + + int bit_acc[N] = {0}; // Buffer of the accelerometer + int get_mag[N] = {0}; // Buffer of the compass + + double calib_acc[N] = {0}; + double calib_gyro_buf[N] = {0}; + double calib_gyro[N] = {0}; + double compass_basis_buf[N] = {0}; + double compass_basis_rad = 0; + double calib_result[7] = {0}; + + double* p_calib_result = calib_result; + + pc.printf("Don't touch... Calibrating now!!\n\r"); + xbee.printf("Don't touch... Calibrating now!!\n\r"); + led1 = 1; + + for( int i=0;i<calib_loop;i++ ){ + + accelerometer.getOutput(bit_acc); + compass.readData(get_mag); + + calib_gyro_buf[0] = gyro.getGyroX(); + calib_gyro_buf[1] = gyro.getGyroY(); + calib_gyro_buf[2] = gyro.getGyroZ(); + + for( int j=0;j<N;j++ ){ + calib_acc[j] += (int16_t)bit_acc[j]; + calib_gyro[j] += calib_gyro_buf[j]; + compass_basis_buf[j] += (int16_t)get_mag[j]; + } + + if( i>calib_loop*3/4 ){ + led4 = 1; + }else if( i>calib_loop/2 ){ + led3 = 1; + }else if( i>calib_loop/4 ){ + led2 = 1; + } + + } + + for( int i=0;i<N;i++ ){ + calib_acc[i] = calib_acc[i]/calib_loop; + calib_gyro[i] = calib_gyro[i]/calib_loop; + compass_basis_buf[i] = compass_basis_buf[i]/calib_loop; + } + + compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0]; + compass_basis_rad = atan(compass_basis_rad); + led1 = 0; led2 = 0; led3 = 0; led4 = 0; + + pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); + xbee.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); + 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); + 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); + + for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; } + for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; } + calib_result[6] = compass_basis_rad; + + if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){ + pc.printf("Accelerometer is not available.\r\n"); + xbee.printf("Accelerometer is not available.\r\n"); + } + + for( int i=0;i<3;i++ ){ + wait(.5); + led1 = 1; led2 = 1; led3 = 1; led4 = 1; + wait(.5); + led1 = 0; led2 = 0; led3 = 0; led4 = 0; + } + + return p_calib_result; + +} \ No newline at end of file