hige dura / Mbed 2 deprecated dotHR_EKF_phithe

Dependencies:   mbed FatFileSystem

main.cpp@1:0c42db451cb9, 2012-09-17 (annotated)

Committer:
higedura
Date:
Mon Sep 17 09:39:54 2012 +0000
Revision:
1:0c42db451cb9
Parent:
0:75227c386257 
dotHR_phothe

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:75227c386257 1 // dot-HR EKF
higedura 0:75227c386257 2
higedura 0:75227c386257 3 #include "mbed.h"
higedura 0:75227c386257 4 #include "SDFileSystem.h"
higedura 0:75227c386257 5 #include "ADXL345_I2C.h"
higedura 0:75227c386257 6 #include "ITG3200.h"
higedura 0:75227c386257 7 #include "HMC5883L.h"
higedura 0:75227c386257 8
higedura 0:75227c386257 9 Serial pc(USBTX, USBRX);
higedura 0:75227c386257 10 DigitalOut led1(LED1);
higedura 0:75227c386257 11 DigitalOut led2(LED2);
higedura 0:75227c386257 12 DigitalOut led3(LED3);
higedura 0:75227c386257 13 DigitalOut led4(LED4);
higedura 0:75227c386257 14 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
higedura 0:75227c386257 15 ADXL345_I2C accelerometer(p9, p10);
higedura 0:75227c386257 16 ITG3200 gyro(p9, p10);
higedura 0:75227c386257 17 HMC5883L compass(p9, p10);
higedura 0:75227c386257 18 Serial xbee(p13, p14);
higedura 0:75227c386257 19 DigitalIn stop(p20);
higedura 0:75227c386257 20 PwmOut ESC1(p21);
higedura 0:75227c386257 21 PwmOut ESC2(p22);
higedura 0:75227c386257 22 PwmOut ESC3(p23);
higedura 0:75227c386257 23 PwmOut ESC4(p24);
higedura 0:75227c386257 24 PwmOut ESC5(p25);
higedura 0:75227c386257 25 PwmOut ESC6(p26);
higedura 0:75227c386257 26 Serial navi(p28, p27); // tx, rx
higedura 0:75227c386257 27 DigitalOut navi_flag(p29);
higedura 0:75227c386257 28
higedura 0:75227c386257 29 #define pi 3.14159265
higedura 0:75227c386257 30
higedura 0:75227c386257 31 #define N 3 // gyro, acc
higedura 0:75227c386257 32 #define N_2 2 // phi, the
higedura 0:75227c386257 33 #define M 6 // The numbers of rotors
higedura 0:75227c386257 34 #define L 5 // The numbers of state quantities (optimal regulator)
higedura 0:75227c386257 35 #define chan 4 // The numbers of channels
higedura 0:75227c386257 36 #define chan_buf 17
higedura 0:75227c386257 37 #define N_LWMA 100
higedura 0:75227c386257 38
higedura 0:75227c386257 39 double* calib();
higedura 0:75227c386257 40 double* RK4( double, double[N_2], double[N_2] );
higedura 0:75227c386257 41 double* func( double[N_2], double[N_2] );
higedura 0:75227c386257 42 double* LWMA( double[N] );
higedura 0:75227c386257 43 double* EKF_predict( double[N_2], double[N_2] );
higedura 0:75227c386257 44 double* EKF_correct( double[N_2], double[N_2], double[N_2] );
higedura 0:75227c386257 45
higedura 0:75227c386257 46 // 0 1 2
higedura 0:75227c386257 47 // [ accXn-1 accXn-2 ... ] 0
higedura 0:75227c386257 48 // zBuf = [ accYn-1 accYn-2 ... ] 1
higedura 0:75227c386257 49 // [ accZn-1 accZn-2 ... ] 2
higedura 0:75227c386257 50 double z_buf[N][N_LWMA] = {0}; // For LWMA
higedura 0:75227c386257 51
higedura 0:75227c386257 52 double P[N_2][N_2] = {{1,0},{0,1}}; // For EKF
higedura 0:75227c386257 53
higedura 0:75227c386257 54 int main(){
higedura 0:75227c386257 55
higedura 0:75227c386257 56 pc.baud(921600);
higedura 1:0c42db451cb9 57 navi.baud(115200);
higedura 0:75227c386257 58 xbee.baud(57600);
higedura 0:75227c386257 59
higedura 1:0c42db451cb9 60 // SD card
higedura 0:75227c386257 61 FILE *fp = fopen("/sd/sdtest.txt", "w");
higedura 0:75227c386257 62 if(fp == NULL) {
higedura 0:75227c386257 63 error("Could not open file for write\n");
higedura 0:75227c386257 64 while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; }
higedura 0:75227c386257 65 }
higedura 0:75227c386257 66
higedura 1:0c42db451cb9 67 int correct_period = 200;
higedura 0:75227c386257 68 int navi_period = 10;
higedura 0:75227c386257 69 int xbee_period = 100;
higedura 0:75227c386257 70 int correct_loop = 0;
higedura 0:75227c386257 71 int navi_loop = 10;
higedura 0:75227c386257 72 int xbee_loop = 100;
higedura 0:75227c386257 73
higedura 1:0c42db451cb9 74 double dt_wait = 0.002; // 100Hz
higedura 0:75227c386257 75 double dt = 0.01;
higedura 0:75227c386257 76 double t = 0;
higedura 0:75227c386257 77
higedura 0:75227c386257 78 int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura 0:75227c386257 79 int get_mag[N] = {0}; // Buffer of the compass
higedura 0:75227c386257 80
higedura 0:75227c386257 81 // Calibration routine
higedura 0:75227c386257 82 double calib_acc[N] = {0};
higedura 0:75227c386257 83 double calib_gyro[N] = {0};
higedura 0:75227c386257 84 //double compass_basis_rad = 0;
higedura 0:75227c386257 85
higedura 0:75227c386257 86 // Getting data
higedura 0:75227c386257 87 double acc[N] = {0, 0, 1};
higedura 0:75227c386257 88 double acc_pre[N] = {0, 0, 1};
higedura 0:75227c386257 89 double d_acc[N] = {0};
higedura 0:75227c386257 90 double gyro_deg[N] = {0};
higedura 0:75227c386257 91 //double gyro_deg_pre[N] = {0};
higedura 0:75227c386257 92 double gyro_rad[N_2] = {0};
higedura 0:75227c386257 93
higedura 0:75227c386257 94 // Measurement algorithm
higedura 0:75227c386257 95 double angle_deg[N_2] = {0};
higedura 0:75227c386257 96 double angle_rad[N_2] = {0};
higedura 0:75227c386257 97 double angle_rad_pre[N_2] = {0};
higedura 0:75227c386257 98 double zLWMA[N] = {0};
higedura 0:75227c386257 99 double zLWMA_2[N_2] = {0};
higedura 0:75227c386257 100
higedura 0:75227c386257 101 // Gravity z
higedura 0:75227c386257 102 for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
higedura 0:75227c386257 103
higedura 0:75227c386257 104 double* p_calib;
higedura 0:75227c386257 105 double* p_RK4;
higedura 0:75227c386257 106 double* p_EKF;
higedura 0:75227c386257 107 double* p_zLWMA;
higedura 0:75227c386257 108
higedura 0:75227c386257 109 // Optimal regulator
higedura 0:75227c386257 110 double state[L];
higedura 1:0c42db451cb9 111 /*
higedura 0:75227c386257 112 // Gain (q1=100, q2=100, q3=50, q4=50, q5=1)
higedura 1:0c42db451cb9 113 double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.408},
higedura 1:0c42db451cb9 114 {-5, 2.887,-3.610, 2.084,-0.408},
higedura 1:0c42db451cb9 115 {-5,-2.887,-3.610,-2.084, 0.408},
higedura 1:0c42db451cb9 116 { 0,-5.774, 0,-4.168,-0.408},
higedura 1:0c42db451cb9 117 { 5,-2.887, 3.610,-2.084, 0.408},
higedura 1:0c42db451cb9 118 { 5, 2.887, 3.610, 2.084,-0.408}};
higedura 1:0c42db451cb9 119 *//*
higedura 0:75227c386257 120 // Gain (q1=100, q2=100, q3=50, q4=50, q5=10)
higedura 1:0c42db451cb9 121 double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.9129},
higedura 1:0c42db451cb9 122 {-5, 2.887,-3.610, 2.084,-0.9129},
higedura 1:0c42db451cb9 123 {-5,-2.887,-3.610,-2.084, 0.9129},
higedura 1:0c42db451cb9 124 { 0,-5.774, 0,-4.168,-0.9129},
higedura 1:0c42db451cb9 125 { 5,-2.887, 3.610,-2.084, 0.9129},
higedura 1:0c42db451cb9 126 { 5, 2.887, 3.610, 2.084,-0.9129}};
higedura 1:0c42db451cb9 127 */
higedura 1:0c42db451cb9 128 double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 17},
higedura 1:0c42db451cb9 129 {-5, 2.887,-3.610, 2.084,-17},
higedura 1:0c42db451cb9 130 {-5,-2.887,-3.610,-2.084, 17},
higedura 1:0c42db451cb9 131 { 0,-5.774, 0,-4.168,-17},
higedura 1:0c42db451cb9 132 { 5,-2.887, 3.610,-2.084, 17},
higedura 1:0c42db451cb9 133 { 5, 2.887, 3.610, 2.084,-17}};
higedura 1:0c42db451cb9 134 /*
higedura 1:0c42db451cb9 135 // Gain (q1=100, q2=100, q3=50, q4=50, q5=10)
higedura 1:0c42db451cb9 136 double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 1.291},
higedura 1:0c42db451cb9 137 {-5, 2.887,-3.610, 2.084,-1.291},
higedura 1:0c42db451cb9 138 {-5,-2.887,-3.610,-2.084, 1.291},
higedura 1:0c42db451cb9 139 { 0,-5.774, 0,-4.168,-1.291},
higedura 1:0c42db451cb9 140 { 5,-2.887, 3.610,-2.084, 1.291},
higedura 1:0c42db451cb9 141 { 5, 2.887, 3.610, 2.084,-1.291}};
higedura 1:0c42db451cb9 142 */
higedura 1:0c42db451cb9 143 // Coefficient for position control
higedura 1:0c42db451cb9 144 double Ccom_r = 0.3;
higedura 1:0c42db451cb9 145 double Ccom_p = 0.3;
higedura 0:75227c386257 146 double Ccom_y = 0.1;
higedura 0:75227c386257 147
higedura 1:0c42db451cb9 148 double motor_control[M] = {0}; // Pulth width generated by the transmitter [s]
higedura 1:0c42db451cb9 149 double motor_attitude[M] = {0}; // Pulth width generated by the attitude control [s]
higedura 1:0c42db451cb9 150 double motor[M] = {0}; // motor_control + motor_attitude [s]
higedura 1:0c42db451cb9 151
higedura 0:75227c386257 152 // Coefficient of transfering Force to Puls
higedura 0:75227c386257 153 double Cfp = 2*pow(10.0, -7);
higedura 0:75227c386257 154
higedura 0:75227c386257 155 // Navigation
higedura 0:75227c386257 156 char command_buf1[chan_buf] = {0};
higedura 0:75227c386257 157 int command_buf2[chan_buf] = {0};
higedura 0:75227c386257 158 double command[chan] = {0};
higedura 0:75227c386257 159 int set_navi_flag = 0;
higedura 0:75227c386257 160
higedura 0:75227c386257 161 // *** Setting up sensors ***
higedura 0:75227c386257 162 //pc.printf("\r\n\r\nSetting up sensors\r\n");
higedura 0:75227c386257 163 xbee.printf("\r\n\r\nSetting up sensors\r\n");
higedura 0:75227c386257 164
higedura 0:75227c386257 165 // These are here to test whether any of the initialization fails. It will print the failure.
higedura 0:75227c386257 166 if (accelerometer.setPowerControl(0x00)){
higedura 0:75227c386257 167 //pc.printf("didn't intitialize power control\n\r");
higedura 0:75227c386257 168 xbee.printf("didn't intitialize power control\n\r");
higedura 0:75227c386257 169 return 0;
higedura 0:75227c386257 170 }
higedura 0:75227c386257 171 // Full resolution, +/-16g, 4mg/LSB.
higedura 0:75227c386257 172 wait(.1);
higedura 0:75227c386257 173
higedura 0:75227c386257 174 if(accelerometer.setDataFormatControl(0x0B)){
higedura 0:75227c386257 175 //pc.printf("didn't set data format\n\r");
higedura 0:75227c386257 176 xbee.printf("didn't set data format\n\r");
higedura 0:75227c386257 177 return 0;
higedura 0:75227c386257 178 }
higedura 0:75227c386257 179 wait(.1);
higedura 0:75227c386257 180
higedura 0:75227c386257 181 // 3.2kHz data rate.
higedura 0:75227c386257 182 if(accelerometer.setDataRate(ADXL345_3200HZ)){
higedura 0:75227c386257 183 //pc.printf("didn't set data rate\n\r");
higedura 0:75227c386257 184 xbee.printf("didn't set data rate\n\r");
higedura 0:75227c386257 185 return 0;
higedura 0:75227c386257 186 }
higedura 0:75227c386257 187 wait(.1);
higedura 0:75227c386257 188
higedura 0:75227c386257 189 if(accelerometer.setPowerControl(MeasurementMode)) {
higedura 0:75227c386257 190 //pc.printf("didn't set the power control to measurement\n\r");
higedura 0:75227c386257 191 xbee.printf("didn't set the power control to measurement\n\r");
higedura 0:75227c386257 192 return 0;
higedura 0:75227c386257 193 }
higedura 0:75227c386257 194 wait(.1);
higedura 0:75227c386257 195
higedura 0:75227c386257 196 gyro.setLpBandwidth(LPFBW_42HZ);
higedura 0:75227c386257 197 compass.setDefault();
higedura 0:75227c386257 198 wait(.1); // Wait some time for all sensors (Need at least 5ms)
higedura 0:75227c386257 199
higedura 0:75227c386257 200 // *** Setting up motors ***
higedura 0:75227c386257 201 //pc.printf("Setting up motors\r\n");
higedura 0:75227c386257 202 xbee.printf("Setting up motors\r\n");
higedura 0:75227c386257 203 ESC1.period(0.016044); ESC2.period(0.016044); ESC3.period(0.016044); ESC4.period(0.016044); ESC5.period(0.016044); ESC6.period(0.016044);
higedura 0:75227c386257 204 // pulsewidth 0.0011~0.00195
higedura 0:75227c386257 205 ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001);
higedura 0:75227c386257 206 // Wait some time for ESC (about 10s)
higedura 0:75227c386257 207 wait(5);
higedura 0:75227c386257 208
higedura 1:0c42db451cb9 209 // *** Calibration routine ***
higedura 1:0c42db451cb9 210 p_calib = calib();
higedura 1:0c42db451cb9 211 for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
higedura 1:0c42db451cb9 212 for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
higedura 1:0c42db451cb9 213 //compass_basis_rad = *p_calib;
higedura 1:0c42db451cb9 214
higedura 0:75227c386257 215 // *** Setting up navigator ***
higedura 0:75227c386257 216 //pc.printf("Setting up navigator\r\n");
higedura 0:75227c386257 217 xbee.printf("Setting up navigator\r\n");
higedura 0:75227c386257 218 while( set_navi_flag==0 ){
higedura 0:75227c386257 219 navi_flag = 1;
higedura 0:75227c386257 220 for( int i=0;i<chan_buf;i++ ){
higedura 0:75227c386257 221 navi.scanf("%c",&command_buf1[i]);
higedura 1:0c42db451cb9 222 //pc.printf("%c",command_buf1[i]);
higedura 0:75227c386257 223 if(command_buf1[i]=='a'){
higedura 0:75227c386257 224 set_navi_flag=1;
higedura 1:0c42db451cb9 225 //pc.printf("\r\nbreak!!\r\n");
higedura 0:75227c386257 226 break;
higedura 0:75227c386257 227 }
higedura 0:75227c386257 228 }
higedura 0:75227c386257 229 navi_flag = 0;
higedura 0:75227c386257 230 wait(.1);
higedura 0:75227c386257 231 }
higedura 1:0c42db451cb9 232
higedura 0:75227c386257 233 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:75227c386257 234
higedura 0:75227c386257 235 //pc.printf("Starting IMU unit\n\r");
higedura 0:75227c386257 236 xbee.printf("Starting IMU unit\n\r");
higedura 0:75227c386257 237 //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");
higedura 0:75227c386257 238 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");
higedura 0:75227c386257 239
higedura 0:75227c386257 240 while( stop==0 ){
higedura 0:75227c386257 241
higedura 0:75227c386257 242 // Navigation
higedura 0:75227c386257 243 if( navi_loop>=navi_period ){
higedura 0:75227c386257 244
higedura 0:75227c386257 245 navi_flag = 1;
higedura 0:75227c386257 246
higedura 0:75227c386257 247 for( int i=0;i<chan_buf;i++ ){ navi.scanf("%c",&command_buf1[i]); }
higedura 0:75227c386257 248 for( int i=0;i<chan_buf;i++ ){
higedura 0:75227c386257 249 command_buf2[i] = (int)command_buf1[i]-48;
higedura 0:75227c386257 250 if( command_buf2[i]==-16 ){ command_buf2[i]=0; }
higedura 1:0c42db451cb9 251 //pc.printf("%d",command_buf2[i]);
higedura 0:75227c386257 252 }
higedura 0:75227c386257 253 command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001;
higedura 0:75227c386257 254 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; }
higedura 0:75227c386257 255
higedura 0:75227c386257 256 navi_flag = 0;
higedura 0:75227c386257 257 navi_loop = 0;
higedura 0:75227c386257 258
higedura 0:75227c386257 259 }
higedura 0:75227c386257 260 navi_loop++;
higedura 0:75227c386257 261
higedura 0:75227c386257 262 // Updating accelerometer and compass
higedura 0:75227c386257 263 accelerometer.getOutput(bit_acc);
higedura 0:75227c386257 264 compass.readData(get_mag);
higedura 0:75227c386257 265
higedura 0:75227c386257 266 // Transfering units and Coordinate transform
higedura 0:75227c386257 267 acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
higedura 0:75227c386257 268 acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
higedura 0:75227c386257 269 acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
higedura 1:0c42db451cb9 270
higedura 0:75227c386257 271 gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0);
higedura 0:75227c386257 272 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
higedura 0:75227c386257 273 gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
higedura 1:0c42db451cb9 274
higedura 0:75227c386257 275 // Limitter for acc
higedura 0:75227c386257 276 if( acc[0]<-1 ){ acc[0] = -1; } if( 1<acc[0] ){ acc[0] = 1; }
higedura 0:75227c386257 277 if( acc[1]<-1 ){ acc[1] = -1; } if( 1<acc[1] ){ acc[1] = 1; }
higedura 0:75227c386257 278 if( acc[2]<-0 ){ acc[2] = -0; } if( 2<acc[2] ){ acc[2] = 2; }
higedura 1:0c42db451cb9 279
higedura 0:75227c386257 280 for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc_pre[i]; }
higedura 0:75227c386257 281 if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc_pre[0]; }
higedura 0:75227c386257 282 if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc_pre[1]; }
higedura 0:75227c386257 283 if( abs(d_acc[2])>=0.5 ){ acc[2] = acc_pre[2]; }
higedura 1:0c42db451cb9 284
higedura 0:75227c386257 285 for( int i=0;i<N_2;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
higedura 1:0c42db451cb9 286
higedura 0:75227c386257 287 // LWMA (Observation)
higedura 0:75227c386257 288 p_zLWMA = LWMA(acc);
higedura 0:75227c386257 289 for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; }
higedura 0:75227c386257 290
higedura 0:75227c386257 291 for( int i=0;i<N_2;i++ ){ zLWMA_2[i] = zLWMA[i]; }
higedura 0:75227c386257 292
higedura 0:75227c386257 293 // RK4 (Prediction)
higedura 0:75227c386257 294 p_RK4 = RK4(dt,angle_rad,gyro_rad);
higedura 0:75227c386257 295 for( int i=0;i<N_2;i++ ){ angle_rad[i] = *p_RK4; p_RK4 = p_RK4+1; }
higedura 0:75227c386257 296
higedura 0:75227c386257 297 // EKF (Correction)
higedura 0:75227c386257 298 EKF_predict(angle_rad,gyro_rad);
higedura 0:75227c386257 299 if ( correct_loop>=correct_period ){
higedura 0:75227c386257 300 p_EKF = EKF_correct(angle_rad,gyro_rad,zLWMA_2);
higedura 0:75227c386257 301 for ( int i=0;i<N_2;i++ ){ angle_rad[i] = *p_EKF; p_EKF = p_EKF+1; }
higedura 0:75227c386257 302 correct_loop = 0;
higedura 0:75227c386257 303 }
higedura 0:75227c386257 304 correct_loop++;
higedura 0:75227c386257 305
higedura 0:75227c386257 306 // Limitter for angle
higedura 0:75227c386257 307 for( int i=0;i<N_2;i++ ){ if( abs(angle_rad[i])>=1.05 ){ angle_rad[i] = angle_rad_pre[i]; } }
higedura 0:75227c386257 308
higedura 0:75227c386257 309 for( int i=0;i<N_2;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; }
higedura 1:0c42db451cb9 310
higedura 0:75227c386257 311 state[0] = angle_deg[0]; state[1] = angle_deg[1];
higedura 0:75227c386257 312 state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2];
higedura 0:75227c386257 313
higedura 0:75227c386257 314 for( int i=0;i<M;i++ ){
higedura 0:75227c386257 315 motor_attitude[i] = 0;
higedura 0:75227c386257 316 for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; }
higedura 0:75227c386257 317 }
higedura 0:75227c386257 318
higedura 0:75227c386257 319 motor_control[0] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]+1*Ccom_y*command[3];
higedura 0:75227c386257 320 motor_control[1] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3];
higedura 0:75227c386257 321 motor_control[2] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3];
higedura 0:75227c386257 322 motor_control[3] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]-1*Ccom_y*command[3];
higedura 0:75227c386257 323 motor_control[4] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3];
higedura 0:75227c386257 324 motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3];
higedura 0:75227c386257 325
higedura 0:75227c386257 326 for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; }
higedura 1:0c42db451cb9 327
higedura 1:0c42db451cb9 328 // pulsewidth 0.0011~0.0019 [s] (0.001~0.002?)
higedura 0:75227c386257 329 for( int i=0;i<M;i++ ){
higedura 1:0c42db451cb9 330 if( motor[i]>0.00195 ){ motor[i]=0.00195; } // Notice! If motor inputs are too large, motors and ESCs may burst.
higedura 1:0c42db451cb9 331 if( motor[i]<0.0012 ){ motor[i]=0.0012; } // Notice! If motor inputs are about 0.0011 [s] for some time, motors and ESCs may burst.
higedura 0:75227c386257 332 if( command[0]<0.0011 ){ motor[i]=0.00105; }
higedura 0:75227c386257 333 }
higedura 1:0c42db451cb9 334
higedura 1:0c42db451cb9 335 // Updating ESC pulse width
higedura 0:75227c386257 336 ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]);
higedura 0:75227c386257 337
higedura 0:75227c386257 338 //for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; }
higedura 0:75227c386257 339
higedura 0:75227c386257 340 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);
higedura 0:75227c386257 341 if( xbee_loop>=xbee_period ){
higedura 0:75227c386257 342 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);
higedura 0:75227c386257 343 xbee_loop = 0;
higedura 0:75227c386257 344 }
higedura 1:0c42db451cb9 345
higedura 0:75227c386257 346 for( int i=0;i<N;i++ ){ acc_pre[i] = acc[i]; }
higedura 0:75227c386257 347 //for( int i=0;i<N;i++ ){ gyro_deg_pre[i] = gyro_deg[i]; }
higedura 0:75227c386257 348 for( int i=0;i<N_2;i++ ){ angle_rad_pre[i] = angle_rad[i]; }
higedura 1:0c42db451cb9 349
higedura 0:75227c386257 350 xbee_loop++;
higedura 0:75227c386257 351 wait(dt_wait);
higedura 0:75227c386257 352 t += dt;
higedura 0:75227c386257 353
higedura 0:75227c386257 354 }
higedura 0:75227c386257 355
higedura 1:0c42db451cb9 356 // pulsewidth 0.0011~0.00195 [s]
higedura 0:75227c386257 357 ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001);
higedura 0:75227c386257 358 led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura 0:75227c386257 359 fclose(fp);
higedura 0:75227c386257 360
higedura 0:75227c386257 361 }
higedura 0:75227c386257 362
higedura 0:75227c386257 363 double* EKF_predict( double y[N_2], double x[N_2] ){
higedura 0:75227c386257 364 // x = F * x;
higedura 0:75227c386257 365 // P = F * P * F' + G * Q * G';
higedura 0:75227c386257 366
higedura 0:75227c386257 367 //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
higedura 0:75227c386257 368 double Q[N_2][N_2] = { {5198, 0}, {0, 5518} };
higedura 0:75227c386257 369
higedura 0:75227c386257 370 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} };
higedura 0:75227c386257 371 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} };
higedura 0:75227c386257 372 double Gjac[N_2][N_2] = { {1, sin(y[0])*tan(y[1])}, {0, cos(y[0])} };
higedura 0:75227c386257 373 double Gjac_t[N_2][N_2] = { {1, 0}, {sin(y[0])*tan(y[1]), cos(y[0])} };
higedura 0:75227c386257 374
higedura 0:75227c386257 375 double FPF[N_2][N_2] = {0};
higedura 0:75227c386257 376 double GQG[N_2][N_2] = {0};
higedura 0:75227c386257 377
higedura 0:75227c386257 378 double FP[N_2][N_2] = {0};
higedura 0:75227c386257 379 double GQ[N_2][N_2] = {0};
higedura 0:75227c386257 380
higedura 0:75227c386257 381 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 382 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 383 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 384 FP[i][j] += Fjac[i][k]*P[k][j];
higedura 0:75227c386257 385 GQ[i][j] += Gjac[i][k]*Q[k][j];
higedura 0:75227c386257 386
higedura 0:75227c386257 387 }
higedura 0:75227c386257 388 }
higedura 0:75227c386257 389 }
higedura 0:75227c386257 390
higedura 0:75227c386257 391 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 392 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 393 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 394 FPF[i][j] += FP[i][k]*Fjac_t[k][j];
higedura 0:75227c386257 395 GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
higedura 0:75227c386257 396 }
higedura 0:75227c386257 397 }
higedura 0:75227c386257 398 }
higedura 0:75227c386257 399 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 400 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 401 P[i][j] = FPF[i][j]+GQG[i][j];
higedura 0:75227c386257 402 }
higedura 0:75227c386257 403 }
higedura 0:75227c386257 404
higedura 0:75227c386257 405 return 0;
higedura 0:75227c386257 406
higedura 0:75227c386257 407 }
higedura 0:75227c386257 408
higedura 0:75227c386257 409 double* EKF_correct( double y[N_2], double x[N_2], double z[N_2] ){
higedura 0:75227c386257 410 // K = P * H' / ( H * P * H' + R )
higedura 0:75227c386257 411 // x = x + K * ( yobs(t) - H * x )
higedura 0:75227c386257 412 // P = P - K * H * P
higedura 0:75227c386257 413
higedura 0:75227c386257 414 //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
higedura 0:75227c386257 415 //double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
higedura 0:75227c386257 416 double R[N_2][N_2] = { {272528, 0}, {0, 295812} };
higedura 0:75227c386257 417
higedura 0:75227c386257 418 double Hjac[N_2][N_2] = { {0, cos(y[1])}, {cos(y[0]), 0} };
higedura 0:75227c386257 419 double Hjac_t[N_2][N_2] = { {0, cos(y[0])}, {cos(y[1]), 0} };
higedura 0:75227c386257 420 double K[N_2][N_2] = {0}; // Kalman gain
higedura 0:75227c386257 421 double K_deno[N_2][N_2] = {0}; // Denominator of the kalman gain
higedura 0:75227c386257 422 double det_K_deno_inv = 0;
higedura 0:75227c386257 423 double K_deno_inv[N_2][N_2] = {0};
higedura 0:75227c386257 424 double HPH[N_2][N_2] = {0};
higedura 0:75227c386257 425 double HP[N_2][N_2] = {0};
higedura 0:75227c386257 426 double PH[N_2][N_2] = {0};
higedura 0:75227c386257 427 double KHP[N_2][N_2] = {0};
higedura 0:75227c386257 428
higedura 0:75227c386257 429 double Hx[N_2] = {0};
higedura 0:75227c386257 430 double z_Hx[N_2] = {0};
higedura 0:75227c386257 431 double Kz_Hx[N_2] = {0};
higedura 0:75227c386257 432
higedura 0:75227c386257 433 double* py = y;
higedura 0:75227c386257 434
higedura 0:75227c386257 435 // Kalman gain
higedura 0:75227c386257 436 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 437 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 438 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 439 HP[i][j] += Hjac[i][k]*P[k][j];
higedura 0:75227c386257 440 PH[i][j] += P[i][k]*Hjac_t[k][j];
higedura 0:75227c386257 441 }
higedura 0:75227c386257 442 }
higedura 0:75227c386257 443 }
higedura 0:75227c386257 444 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 445 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 446 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 447 HPH[i][j] += HP[i][k]*Hjac_t[k][j];
higedura 0:75227c386257 448 }
higedura 0:75227c386257 449 }
higedura 0:75227c386257 450 }
higedura 0:75227c386257 451 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 452 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 453 K_deno[i][j] = HPH[i][j]+R[i][j];
higedura 0:75227c386257 454 }
higedura 0:75227c386257 455 }
higedura 0:75227c386257 456 // Inverce matrix
higedura 0:75227c386257 457 det_K_deno_inv = K_deno[0][0]*K_deno[1][1]-K_deno[1][0]*K_deno[0][1];
higedura 0:75227c386257 458 K_deno_inv[0][0] = K_deno[1][1]/det_K_deno_inv;
higedura 0:75227c386257 459 K_deno_inv[0][1] = -K_deno[0][1]/det_K_deno_inv;
higedura 0:75227c386257 460 K_deno_inv[1][0] = -K_deno[1][0]/det_K_deno_inv;
higedura 0:75227c386257 461 K_deno_inv[1][1] = K_deno[0][0]/det_K_deno_inv;
higedura 0:75227c386257 462
higedura 0:75227c386257 463 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 464 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 465 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 466 K[i][j] += PH[i][k]*K_deno_inv[k][j];
higedura 0:75227c386257 467 }
higedura 0:75227c386257 468 }
higedura 0:75227c386257 469 }
higedura 0:75227c386257 470
higedura 0:75227c386257 471 // Filtering
higedura 0:75227c386257 472 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 473 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 474 Hx[i] += Hjac[i][j]*y[j];
higedura 0:75227c386257 475 }
higedura 0:75227c386257 476 }
higedura 0:75227c386257 477 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 478 z_Hx[i] = z[i]-Hx[i];
higedura 0:75227c386257 479 }
higedura 0:75227c386257 480 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 481 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 482 Kz_Hx[i] += K[i][j]*z_Hx[j];
higedura 0:75227c386257 483 }
higedura 0:75227c386257 484 }
higedura 0:75227c386257 485 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 486 y[i] = y[i]+Kz_Hx[i];
higedura 0:75227c386257 487 }
higedura 0:75227c386257 488
higedura 0:75227c386257 489 // Covarience
higedura 0:75227c386257 490 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 491 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 492 for( int k=0;k<N_2;k++ ){
higedura 0:75227c386257 493 KHP[i][j] += K[i][k]*HP[k][j];
higedura 0:75227c386257 494 }
higedura 0:75227c386257 495 }
higedura 0:75227c386257 496 }
higedura 0:75227c386257 497 for( int i=0;i<N_2;i++ ){
higedura 0:75227c386257 498 for( int j=0;j<N_2;j++ ){
higedura 0:75227c386257 499 P[i][j] = P[i][j]-KHP[i][j];
higedura 0:75227c386257 500 }
higedura 0:75227c386257 501 }
higedura 0:75227c386257 502
higedura 0:75227c386257 503 return py;
higedura 0:75227c386257 504
higedura 0:75227c386257 505 }
higedura 0:75227c386257 506
higedura 0:75227c386257 507 double* LWMA( double z[N] ){
higedura 0:75227c386257 508
higedura 0:75227c386257 509 double zLWMA[N] = {0};
higedura 0:75227c386257 510 double zLWMA_num[N] = {0};
higedura 0:75227c386257 511 double zLWMA_den = 0;
higedura 0:75227c386257 512
higedura 0:75227c386257 513 double* pzLWMA = zLWMA;
higedura 0:75227c386257 514
higedura 0:75227c386257 515 for( int i=1;i<N_LWMA;i++ ){
higedura 0:75227c386257 516 for( int j=0;j<N;j++ ){
higedura 0:75227c386257 517 z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
higedura 0:75227c386257 518 }
higedura 0:75227c386257 519 }
higedura 0:75227c386257 520 for( int i=0;i<N;i++ ){
higedura 0:75227c386257 521 z_buf[i][0] = z[i];
higedura 0:75227c386257 522 }
higedura 0:75227c386257 523
higedura 0:75227c386257 524 for( int i=0;i<N_LWMA;i++ ){
higedura 0:75227c386257 525 for( int j=0;j<N;j++ ){
higedura 0:75227c386257 526 zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
higedura 0:75227c386257 527 }
higedura 0:75227c386257 528 zLWMA_den += N_LWMA-i;
higedura 0:75227c386257 529 }
higedura 0:75227c386257 530 for( int i=0;i<N;i++ ){
higedura 0:75227c386257 531 zLWMA[i] = zLWMA_num[i]/zLWMA_den;
higedura 0:75227c386257 532 }
higedura 0:75227c386257 533
higedura 0:75227c386257 534 return pzLWMA;
higedura 0:75227c386257 535
higedura 0:75227c386257 536 }
higedura 0:75227c386257 537
higedura 0:75227c386257 538 double* RK4( double dt, double y[N_2], double x[N_2] ){
higedura 0:75227c386257 539
higedura 0:75227c386257 540 double yBuf[N_2] = {0};
higedura 0:75227c386257 541 double k[N_2][4] = {0};
higedura 0:75227c386257 542
higedura 0:75227c386257 543 double* p_y = y;
higedura 0:75227c386257 544
higedura 0:75227c386257 545 double* pk1;
higedura 0:75227c386257 546 double* pk2;
higedura 0:75227c386257 547 double* pk3;
higedura 0:75227c386257 548 double* pk4;
higedura 0:75227c386257 549
higedura 0:75227c386257 550 for( int i=0;i<N_2;i++){ yBuf[i] = y[i]; }
higedura 0:75227c386257 551 pk1 = func (yBuf,x);
higedura 0:75227c386257 552 for( int i=0;i<N_2;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
higedura 0:75227c386257 553
higedura 0:75227c386257 554 for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
higedura 0:75227c386257 555 pk2 = func (yBuf,x);
higedura 0:75227c386257 556 for( int i=0;i<N_2;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
higedura 0:75227c386257 557
higedura 0:75227c386257 558 for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
higedura 0:75227c386257 559 pk3 = func (yBuf,x);
higedura 0:75227c386257 560 for( int i=0;i<N_2;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
higedura 0:75227c386257 561
higedura 0:75227c386257 562 for( int i=0;i<N_2;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
higedura 0:75227c386257 563 pk4 = func (yBuf,x);
higedura 0:75227c386257 564 for( int i=0;i<N_2;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
higedura 0:75227c386257 565
higedura 0:75227c386257 566 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; }
higedura 0:75227c386257 567
higedura 0:75227c386257 568 return p_y;
higedura 0:75227c386257 569
higedura 0:75227c386257 570 }
higedura 0:75227c386257 571
higedura 0:75227c386257 572 double* func( double y[N_2], double x[N_2] ){
higedura 0:75227c386257 573
higedura 0:75227c386257 574 double f[N_2] = {0};
higedura 0:75227c386257 575 double* p_f = f;
higedura 0:75227c386257 576 double a;
higedura 0:75227c386257 577 double b;
higedura 0:75227c386257 578
higedura 0:75227c386257 579 f[0] = x[0]+x[1]*sin(y[0])*tan(y[1]);
higedura 0:75227c386257 580 f[1] = x[1]*cos(y[0]);
higedura 1:0c42db451cb9 581 // Without a, f[1] is not passed main function for unknown reason.
higedura 0:75227c386257 582 a = sin(b);
higedura 0:75227c386257 583
higedura 0:75227c386257 584 return p_f;
higedura 0:75227c386257 585
higedura 0:75227c386257 586 }
higedura 0:75227c386257 587
higedura 0:75227c386257 588 double* calib(){
higedura 0:75227c386257 589
higedura 0:75227c386257 590 int calib_loop = 1000;
higedura 0:75227c386257 591
higedura 0:75227c386257 592 int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura 0:75227c386257 593 int get_mag[N] = {0}; // Buffer of the compass
higedura 0:75227c386257 594
higedura 0:75227c386257 595 double calib_acc[N] = {0};
higedura 0:75227c386257 596 double calib_gyro_buf[N] = {0};
higedura 0:75227c386257 597 double calib_gyro[N] = {0};
higedura 0:75227c386257 598 double compass_basis_buf[N] = {0};
higedura 0:75227c386257 599 double compass_basis_rad = 0;
higedura 0:75227c386257 600 double calib_result[7] = {0};
higedura 0:75227c386257 601
higedura 0:75227c386257 602 double* p_calib_result = calib_result;
higedura 0:75227c386257 603
higedura 1:0c42db451cb9 604 //pc.printf("Don't touch... Calibrating now!!\n\r");
higedura 0:75227c386257 605 xbee.printf("Don't touch... Calibrating now!!\n\r");
higedura 0:75227c386257 606 led1 = 1;
higedura 0:75227c386257 607
higedura 0:75227c386257 608 for( int i=0;i<calib_loop;i++ ){
higedura 0:75227c386257 609
higedura 0:75227c386257 610 accelerometer.getOutput(bit_acc);
higedura 0:75227c386257 611 compass.readData(get_mag);
higedura 0:75227c386257 612
higedura 0:75227c386257 613 calib_gyro_buf[0] = gyro.getGyroX();
higedura 0:75227c386257 614 calib_gyro_buf[1] = gyro.getGyroY();
higedura 0:75227c386257 615 calib_gyro_buf[2] = gyro.getGyroZ();
higedura 0:75227c386257 616
higedura 0:75227c386257 617 for( int j=0;j<N;j++ ){
higedura 0:75227c386257 618 calib_acc[j] += (int16_t)bit_acc[j];
higedura 0:75227c386257 619 calib_gyro[j] += calib_gyro_buf[j];
higedura 0:75227c386257 620 compass_basis_buf[j] += (int16_t)get_mag[j];
higedura 0:75227c386257 621 }
higedura 0:75227c386257 622
higedura 0:75227c386257 623 if( i>calib_loop*3/4 ){
higedura 0:75227c386257 624 led4 = 1;
higedura 0:75227c386257 625 }else if( i>calib_loop/2 ){
higedura 0:75227c386257 626 led3 = 1;
higedura 0:75227c386257 627 }else if( i>calib_loop/4 ){
higedura 0:75227c386257 628 led2 = 1;
higedura 0:75227c386257 629 }
higedura 0:75227c386257 630
higedura 0:75227c386257 631 }
higedura 0:75227c386257 632
higedura 0:75227c386257 633 for( int i=0;i<N;i++ ){
higedura 0:75227c386257 634 calib_acc[i] = calib_acc[i]/calib_loop;
higedura 0:75227c386257 635 calib_gyro[i] = calib_gyro[i]/calib_loop;
higedura 0:75227c386257 636 compass_basis_buf[i] = compass_basis_buf[i]/calib_loop;
higedura 0:75227c386257 637 }
higedura 0:75227c386257 638
higedura 0:75227c386257 639 compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0];
higedura 0:75227c386257 640 compass_basis_rad = atan(compass_basis_rad);
higedura 0:75227c386257 641 led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura 0:75227c386257 642
higedura 1:0c42db451cb9 643 //pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
higedura 0:75227c386257 644 xbee.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
higedura 1:0c42db451cb9 645 //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);
higedura 0:75227c386257 646 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);
higedura 0:75227c386257 647
higedura 0:75227c386257 648 for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; }
higedura 0:75227c386257 649 for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; }
higedura 0:75227c386257 650 calib_result[6] = compass_basis_rad;
higedura 0:75227c386257 651
higedura 0:75227c386257 652 if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){
higedura 1:0c42db451cb9 653 //pc.printf("Accelerometer is not available.\r\n");
higedura 0:75227c386257 654 xbee.printf("Accelerometer is not available.\r\n");
higedura 0:75227c386257 655 }
higedura 0:75227c386257 656
higedura 0:75227c386257 657 for( int i=0;i<3;i++ ){
higedura 0:75227c386257 658 wait(.5);
higedura 0:75227c386257 659 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:75227c386257 660 wait(.5);
higedura 0:75227c386257 661 led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura 0:75227c386257 662 }
higedura 0:75227c386257 663
higedura 0:75227c386257 664 return p_calib_result;
higedura 0:75227c386257 665
higedura 0:75227c386257 666 }