main.cpp

00001 #include "mbed.h"
00002 #include "Barometer.h"
00003 #include "LocalFileSystem.h"
00004 #include "math.h"
00005 #include "Servo.h"
00006 #define dt 0.1
00007 
00008 Serial pc(USBTX, USBRX);
00009 Ticker blue_trig;
00010 Timer end;
00011 
00012 ////////////////////////////////////////
00013 //   Bluetooth    3.3V p13 p14(TX,RX) //
00014 ////////////////////////////////////////
00015 Serial Blue(p13, p14);
00016 int send_ok=0;
00017 char Blue_msg[150];
00018 int k=0, Blue_ok=0, Blue_flag=0;
00019 volatile unsigned char Blue_buffer[2];
00020 float input_roll, input_pitch, input_yaw, input_thr;
00021 
00022 void Blue_isr(){                      //inturupt 
00023     while(Blue.readable()){
00024         Blue_buffer[1] = Blue_buffer[0];
00025         Blue_buffer[0] = Blue.getc();
00026         if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
00027         if (Blue_buffer[0] == '*'){Blue_flag=1;}
00028         if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
00029     }
00030 } 
00031 
00032 void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
00033 {
00034     if (Blue_ok == 1){
00035         Blue_ok = 0;
00036         sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
00037     }
00038 } 
00039 
00040 void blue_trig_isr(){
00041     send_ok=1;   
00042 }
00043 
00044 void trans_blue_data(float in_data, int integer_point, int under_point){  // number of intefer and under_point
00045         unsigned int conv_trans_data;
00046         conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
00047         if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
00048         else {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '+';}
00049         for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
00050             if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.';   }        
00051             while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48);       //convert to ASCII    
00052         }
00053 }
00054 
00055 //Bluetooth code is placed under the Log_data
00056 
00057 ////////////////////////////////////////////
00058 //    Barometer     3.3V p9(SDA) p10(SCL) //  
00059 ////////////////////////////////////////////
00060 Barometer barometer(p9, p10);
00061 float alt=0.0;
00062 float alt_sum=0.0f, alt_zero=0.0f;
00063 int count = 0, baro_ok = 0;          // for zero-calibration
00064 float alt_buffer[2], w_alt=0;          // weight for LPF
00065 
00066 void get_Baro(float*alt)
00067 {   
00068     if (baro_ok==1){
00069         barometer.update();
00070         *alt = barometer.get_altitude_m();
00071         alt_buffer[1] = alt_buffer[0]; 
00072         alt_buffer[0] = *alt;
00073         if(abs(alt_buffer[0]- alt_buffer[1])>20){
00074             *alt = alt_buffer[1];
00075             baro_ok = 0;
00076         }
00077         else{
00078             baro_ok = 0;
00079         }
00080     }
00081 } 
00082 
00083 void calb_alt(){
00084     if (alt==0){count=0;}
00085     else {
00086         if (count==1){count++;}
00087         else{
00088             if (count<=99){alt_sum = alt_sum + alt; count++;}
00089             else {alt_zero = alt_sum/(float)(count-1); count++;}
00090         }
00091     }
00092 }
00093 
00094 ///////////////////////////////////////
00095 //         AHRS          5V p27(RX)  //   20Hz
00096 ///////////////////////////////////////
00097 Serial AHRS(p28, p27);
00098 float roll,pitch,yaw,velx,vely,velz,velxyz;
00099 char AHRS_msg[150];
00100 int m=0, ahrs_ok=0, AHRS_flag=0;
00101 volatile unsigned char AHRS_buffer[2];
00102 
00103 void AHRS_isr(){                      //inturupt 
00104     while(AHRS.readable()){
00105         AHRS_buffer[1] = AHRS_buffer[0];
00106         AHRS_buffer[0] = AHRS.getc();
00107         if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
00108         if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
00109         if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
00110     }
00111 } 
00112 
00113 void get_AHRS(float*roll, float*pitch, float*yaw, float*velx, float*vely, float*velz, float*velxyz)
00114 {
00115     if (ahrs_ok == 1){
00116         ahrs_ok = 0;
00117         sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
00118         *velxyz = (float)sqrt(pow(*velx,2)+pow(*vely,2)+pow(*velz,2));
00119         baro_ok = 1;
00120     }
00121 } 
00122 
00123 ///////////////////////////////////
00124 //      Servo         5V  PWM    // needed to check pin#
00125 ///////////////////////////////////
00126 Servo Throttle(p26);
00127 Servo Ctrl_1(p25); //below
00128 Servo Ctrl_2(p23); //upper
00129 Servo Ctrl_3(p21); //below
00130 Servo Ctrl_4(p22); //upper
00131 
00132 float thr_value = 0.0, cotrl1_value = 0.5, cotrl2_value = 0.5, cotrl3_value = 0.5, cotrl4_value = 0.5;
00133 float err_roll = 0.0, err_pitch = 0.0, err_yaw = 0.0, err_alt = 0.0, pre_er = 0.0, pre_ep = 0.0, pre_ey = 0.0;
00134 float kp1=1.0, kd1=0.0, kp2=1.0, kd2=0.0, kp3=1.0, kd3=0.0;   // PD Controller
00135 
00136 void ctrl_roll(){
00137     err_roll = input_roll - roll;
00138     cotrl1_value = (kp1*err_roll)+(kd1*(err_roll-pre_er)/dt);
00139     pre_er = err_roll;
00140     cotrl1_value = (cotrl1_value/180.0) + 0.5;
00141     if (cotrl1_value<=0.0){cotrl1_value=0.0;}
00142     else if (cotrl1_value>=1.0) {cotrl1_value=1.0;}
00143     Ctrl_1 = cotrl1_value;
00144     Ctrl_3 = -cotrl1_value;
00145 }
00146 
00147 void ctrl_pitch(){
00148     err_pitch = input_pitch - pitch;
00149     cotrl2_value = (kp2*err_pitch)+(kd2*(err_pitch-pre_ep)/dt);
00150     pre_ep = err_pitch;
00151     cotrl2_value = (cotrl2_value/180.0) + 0.5;
00152     if (cotrl2_value<=0.0){cotrl2_value=0.0;}
00153     else if (cotrl2_value>=1.0) {cotrl2_value=1.0;}
00154     Ctrl_2 = cotrl2_value;
00155     Ctrl_4 = -cotrl2_value;
00156 }
00157   
00158 void ctrl_yaw(){
00159     err_yaw = input_yaw - yaw;
00160     cotrl3_value = (kp3*err_yaw)+(kd3*(err_yaw-pre_ey)/dt);
00161     pre_ey = err_yaw;
00162     cotrl3_value = (cotrl3_value/180.0) + 0.5;
00163     if (cotrl3_value<=0.0){cotrl3_value=0.0;}
00164     else if (cotrl3_value>=1.0) {cotrl3_value=1.0;}
00165     Ctrl_1 = cotrl3_value;
00166     Ctrl_2 = cotrl3_value;
00167     Ctrl_3 = cotrl3_value;
00168     Ctrl_4 = cotrl3_value;
00169 }
00170 
00171 void neutral(){
00172     Ctrl_1 = cotrl1_value;
00173     Ctrl_2 = cotrl2_value;
00174     Ctrl_3 = cotrl3_value;
00175     Ctrl_4 = cotrl4_value;
00176 }
00177 
00178 ///////////////////////////////
00179 //   Datalogger       Mbed   //
00180 ///////////////////////////////
00181 LocalFileSystem local("local");
00182 int file_no=1;
00183 char filename[256];
00184 FILE *fp;
00185 
00186 void Log_file(){
00187     sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
00188     fp = fopen(filename, "r"); // if file can be loaded, return is 1
00189     while(fp){
00190         fclose(fp);
00191         file_no ++;
00192         sprintf(filename, "/local/Data%d.txt", file_no);    // Open "tem%d.txt" on the local file system for writing
00193         fp = fopen(filename, "r");
00194     }
00195     fp = fopen(filename, "w");    
00196 }
00197 
00198 void Log_data(){
00199     fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",roll,pitch,yaw,alt,input_roll,input_pitch,input_yaw,input_thr,velx,vely,velz);
00200 }
00201 
00202 void send_Blue(){
00203     if (send_ok == 1){
00204         send_ok = 0;
00205         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
00206         trans_blue_data(roll,3,2);
00207         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
00208         trans_blue_data(pitch,3,2);
00209         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
00210         trans_blue_data(yaw,3,2);
00211         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
00212         trans_blue_data(alt,2,2);
00213         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
00214         trans_blue_data(velxyz,2,2);
00215         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
00216         trans_blue_data(velz,2,2);
00217         while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
00218     }
00219 }
00220 
00221 
00222 /////////////////////////////////
00223 //         Main loop           // 
00224 /////////////////////////////////
00225 
00226 int main(void)
00227 {   
00228     AHRS.baud(9600);
00229     Blue.baud(9600);
00230     Blue.attach(&Blue_isr);
00231     AHRS.attach(&AHRS_isr);
00232     blue_trig.attach(&blue_trig_isr, 0.1);
00233     neutral();
00234     //Log_file();
00235         while(1) {
00236                 get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
00237                 get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
00238                 get_Baro(&alt);
00239                 if (input_roll!=0){ctrl_roll();}
00240                 if (input_pitch!=0){ctrl_pitch();}
00241                 if (input_yaw!=0){ctrl_yaw();}
00242                 send_Blue();
00243                 if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){break;}
00244     }
00245 }