Kim Youngsik
/
mbed_droptest2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
mbed_droptest
by 
Soyoon Kim
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "Barometer.h" 00003 #include "LocalFileSystem.h" 00004 #include "math.h" 00005 #include "Servo.h" 00006 00007 Serial pc(USBTX, USBRX); 00008 Timer end; 00009 int stat=1; 00010 00011 volatile unsigned char tmp; 00012 void pc_isr(){ 00013 __disable_irq(); 00014 while(pc.readable()){ 00015 tmp = pc.getc(); 00016 } 00017 __enable_irq(); 00018 } 00019 00020 00021 //////////////////////////////////// 00022 // GPS 5V p27(RX) // 00023 // Bluetooth 3.3V p28(TX) // 00024 //////////////////////////////////// 00025 Serial Blue_GPS(p28, p27); 00026 00027 char GPS_msg[150], ns, ew; 00028 int i = 0, j=0, k=0, gps_ok=0, GPS_flag; 00029 volatile unsigned char GPS_buffer[2]; 00030 float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time; 00031 int blue_ok=0; 00032 00033 void GPS_isr(){ 00034 while(Blue_GPS.readable()){ 00035 i++; 00036 GPS_buffer[1] = GPS_buffer[0]; 00037 GPS_buffer[0] = Blue_GPS.getc(); 00038 if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){ 00039 i=0; 00040 if (GPS_flag == 1){GPS_flag = 0; gps_ok = 1; j=0;} 00041 } 00042 if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;} 00043 if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++; } 00044 // pc.putc(GPS_buffer[0]); 00045 // Blue_GPS.putc(GPS_buffer[0]); 00046 } 00047 return; 00048 } 00049 float trunc(float v) { 00050 if(v < 0.0) { 00051 v*= -1.0; 00052 v = floor(v); 00053 v*=-1.0; 00054 } 00055 else {v = floor(v);} 00056 return v; 00057 } 00058 00059 void get_GPS(float *Kor_time, float *latitude, char *ns, float *longitude, char *ew, float *fix, float *sat, float *x, float *alt_GPS, float *lock) 00060 { 00061 if (gps_ok == 1){ 00062 gps_ok = 0; 00063 sscanf(GPS_msg, "GA,%f,%f,%c,%f,%c,%f,%f,%f,%f,%f", Kor_time,latitude,ns,longitude,ew,fix,sat,x,alt_GPS,lock); 00064 if(*ns == 'S') {*latitude *= -1.0; } 00065 if(*ew == 'W') {*longitude *= -1.0; } 00066 float degrees = trunc(*latitude / 100.0f); 00067 float minutes = (*latitude - (degrees * 100.0f)); 00068 *latitude = degrees + minutes / 60.0f; 00069 degrees = trunc(*longitude / 100.0f); 00070 minutes = *longitude - (degrees * 100.0f); 00071 *longitude = degrees + minutes / 60.0f; 00072 *Kor_time = *Kor_time + 90000; 00073 } 00074 } 00075 00076 //Bluetooth code is placed under the Log_data 00077 00078 //////////////////////////////////////////// 00079 // Barometer 3.3V p9(SDA) p10(SCL) // 00080 //////////////////////////////////////////// 00081 Barometer barometer(p9, p10); 00082 float t, alt, del_alt; 00083 float alt_sum=0.0f, alt_zero=0.0f; 00084 int count = 0, baro_ok = 0; // for zero-calibration 00085 float alt_buffer[4], w_alt=0.6; // weight for LPF 00086 00087 void get_Baro(float*t, float*alt, float*del_alt) 00088 { 00089 if (baro_ok==1){ 00090 barometer.update(); 00091 *t = barometer.get_temperature(); 00092 *alt = barometer.get_altitude_m(); 00093 alt_buffer[1] = alt_buffer[0]; 00094 alt_buffer[0] = *alt; 00095 *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter 00096 alt_buffer[3] = alt_buffer[2]; 00097 alt_buffer[2] = *alt; 00098 *del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed 00099 baro_ok = 0; 00100 } 00101 } 00102 00103 void calb_alt(){ 00104 if (alt==0){} 00105 else { 00106 if (count<=19){alt_sum = alt_sum + alt; count++;} 00107 else { 00108 // Blue_GPS.printf("calibration\r\n"); 00109 alt_zero = alt_sum / (float)count; 00110 count++; 00111 } 00112 } 00113 } 00114 /* 00115 void lpf_alt(){ 00116 alt_buffer[1] = alt_buffer[0]; 00117 alt_buffer[0] = alt; 00118 alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter 00119 alt_buffer[3] = alt_buffer[2]; 00120 alt_buffer[2] = alt; 00121 del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed 00122 } 00123 */ 00124 /////////////////////////////////////// 00125 // AHRS 5V p14(RX) // 20Hz 00126 /////////////////////////////////////// 00127 Serial AHRS(p13, p14); 00128 float roll,pitch,yaw,accx,accy,accz; 00129 char AHRS_msg[150]; 00130 int m=0, ahrs_ok=0, AHRS_flag; 00131 volatile unsigned char AHRS_buffer[2]; 00132 float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8; 00133 00134 void AHRS_isr(){ //inturupt 00135 __disable_irq(); 00136 while(AHRS.readable()){ 00137 AHRS_buffer[1] = AHRS_buffer[0]; 00138 AHRS_buffer[0] = AHRS.getc(); 00139 if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;} 00140 if (AHRS_buffer[0] == '*'){AHRS_flag=1;} 00141 if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;} 00142 } 00143 __enable_irq(); 00144 } 00145 00146 void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz) 00147 { 00148 if (ahrs_ok == 1){ 00149 ahrs_ok = 0; 00150 sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz); 00151 roll_buffer[1] = roll_buffer[0]; 00152 roll_buffer[0] = *roll; 00153 *roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter 00154 pitch_buffer[1] = pitch_buffer[0]; 00155 pitch_buffer[0] = *pitch; 00156 *pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter 00157 yaw_buffer[1] = yaw_buffer[0]; 00158 yaw_buffer[0] = *yaw; 00159 *yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter 00160 baro_ok = 1; 00161 blue_ok = 1; 00162 } 00163 } 00164 /* 00165 void lpf_attitude(){ 00166 roll_buffer[1] = roll_buffer[0]; 00167 roll_buffer[0] = roll; 00168 roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter 00169 pitch_buffer[1] = pitch_buffer[0]; 00170 pitch_buffer[0] = pitch; 00171 pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter 00172 yaw_buffer[1] = yaw_buffer[0]; 00173 yaw_buffer[0] = yaw; 00174 yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter 00175 } 00176 */ 00177 00178 /////////////////////////////////// 00179 // Servo 5V PWM // 00180 /////////////////////////////////// 00181 Servo Micro_gf(p22); 00182 00183 float unf_value=1.0, gf_value = 0.35,linear_value = 1.0; 00184 float tg_yaw = 0.0, err_yaw = 0.0, p=1.5; 00185 void ctl_gf(){ 00186 err_yaw = yaw - tg_yaw; 00187 gf_value = 0.35*exp(0.0039*p*err_yaw); 00188 if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));} 00189 if (gf_value<=0.0){gf_value=0.0;} 00190 else if (gf_value>=0.7) {gf_value=0.7;} 00191 Micro_gf = gf_value; 00192 } 00193 00194 /////////////////////////////// 00195 // Datalogger Mbed // 00196 /////////////////////////////// 00197 LocalFileSystem local("local"); 00198 int file_no=1; 00199 char filename[256]; 00200 FILE *fp; 00201 00202 void Log_file(){ 00203 sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing 00204 fp = fopen(filename, "r"); // if file can be loaded, return is 1 00205 while(fp){ 00206 fclose(fp); 00207 file_no ++; 00208 sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing 00209 fp = fopen(filename, "r"); 00210 } 00211 fp = fopen(filename, "w"); 00212 } 00213 00214 void Log_data(){ 00215 fprintf(fp, "%i,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value); 00216 } 00217 00218 00219 ///////////////////////////New Add///////////////// 00220 void trans_blue_data(float in_data, int integer_point, int under_point){ // number of intefer and under_point 00221 unsigned int conv_trans_data; 00222 conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point)); 00223 if(in_data<0) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '-';} 00224 for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){ 00225 if(cnt_num == under_point) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '.'; } 00226 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48); //convert to ASCII 00227 } 00228 } 00229 00230 /////////////////////////////////////////////////////////////////////////////// 00231 00232 void send_Blue(){ 00233 if (blue_ok == 1){ 00234 blue_ok = 0; 00235 trans_blue_data(latitude,2,6); 00236 // Blue_GPS.printf("%i,%.0f,%.6f,%.6f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,del_alt,roll,pitch,yaw,accz,unf_value,gf_value,linear_value); 00237 } 00238 } 00239 void send_PC(){ 00240 pc.printf("%i,%.0f,%.6f,%.6f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,del_alt,roll,pitch,yaw,accz,unf_value,gf_value,linear_value); 00241 } 00242 00243 Ticker timer1; 00244 Ticker timer2; 00245 bool gpsFlag = 0; 00246 bool blueFlag = 0; 00247 void timer1_isr(){ 00248 gpsFlag = 1; 00249 } 00250 void timer2_isr(){ 00251 blueFlag = 1; 00252 } 00253 00254 /////////////////////////////////////////////// 00255 00256 ///////////////////////////////// 00257 // Main loop // 00258 ///////////////////////////////// 00259 00260 int main(void) 00261 { 00262 AHRS.baud(9600); 00263 Blue_GPS.baud(9600); 00264 pc.baud(9600); 00265 /* 00266 while(Blue_GPS.readable()){ 00267 volatile unsigned char temp = Blue_GPS.getc(); 00268 } 00269 Blue_GPS.printf("Buffer flushing\r\n"); 00270 */ 00271 Blue_GPS.attach(&GPS_isr); 00272 pc.attach(&pc_isr); 00273 AHRS.attach(&AHRS_isr); 00274 // Blue_GPS.printf("Start\r\n"); 00275 //Log_file(); 00276 timer1.attach(&timer1_isr, 1.0); 00277 timer2.attach(&timer2_isr, 0.1); 00278 gf_value = 0.7; 00279 Micro_gf = gf_value; 00280 while(1) { 00281 00282 switch(stat){ 00283 case 1 : //Calibration 00284 if(gpsFlag) 00285 { 00286 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock); 00287 // Blue_GPS.printf("getGPS\r\n"); 00288 00289 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 'g'; 00290 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 'e'; 00291 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 't'; 00292 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 'G'; 00293 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 'P'; 00294 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 'S'; 00295 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 13; 00296 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 10; 00297 00298 gpsFlag = 0; 00299 } 00300 if(blueFlag) 00301 { 00302 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz); 00303 get_Baro(&t, &alt, &del_alt); 00304 calb_alt(); 00305 // Blue_GPS.printf("%i,%.2f,%.2f,%.2f\r\n",count, alt, alt_sum, alt_zero); 00306 //////Count///////// 00307 trans_blue_data((float)count,3,0); 00308 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 00309 /////////Alt/////// 00310 00311 ////////Data Trans to Bluetooth /// 00312 trans_blue_data(alt,3,6); 00313 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 00314 trans_blue_data(alt_sum,6,2); 00315 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 00316 // float_2_ggaji(alt_zero); 00317 trans_blue_data(alt_zero, 5, 3); 00318 //////////Line Feed// 00319 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 13; 00320 while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = 10; 00321 blueFlag = 0; 00322 00323 } 00324 if (20<count) { 00325 stat=2; 00326 } 00327 break; 00328 case 2 : //Wait 00329 if(gpsFlag) 00330 { 00331 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock); 00332 gpsFlag = 0; 00333 } 00334 00335 if(blueFlag) 00336 { 00337 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz); 00338 get_Baro(&t, &alt, &del_alt); 00339 //Log_data(); 00340 alt = alt - alt_zero; 00341 send_Blue(); 00342 send_PC(); 00343 blueFlag = 0; 00344 } 00345 if (alt<=-10 && abs(accx)<0.1 && abs(accy)<0.1){ 00346 //fclose(fp); 00347 stat=3; send_Blue(); 00348 } 00349 break; 00350 00351 /*case 3 : //Drop 00352 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock); 00353 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz); 00354 get_Baro(&t, &alt); 00355 //Log_data(); 00356 lpf_alt(); 00357 alt = alt - alt_zero; 00358 lpf_attitude(); 00359 send_Blue(); 00360 pc.printf("%i,%.2f,%.2f,%.2f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value); 00361 if (alt<=5 && abs(accx)<0.1 && abs(accy)<0.1) {end.start(); stat=4;} 00362 break; 00363 case 4 : //Landing 00364 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz); 00365 get_Baro(&t, &alt); 00366 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock); 00367 //Log_data(); 00368 lpf_alt(); 00369 alt = alt - alt_zero; 00370 lpf_attitude(); 00371 send_Blue(); 00372 end.read(); 00373 pc.printf("%i,%.2f,%.2f,%.2f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value); 00374 if (end.read()>=10) {fclose(fp);} 00375 break;*/ 00376 case 3 : //Shut down 00377 break; 00378 } 00379 } 00380 } 00381
Generated on Thu Sep 1 2022 23:10:51 by
1.7.2