Soyoon Kim / Mbed 2 deprecated mbed_droptest

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.

Dependencies:   Servo mbed

main.cpp@3:880d00d555c1, 2016-08-01 (annotated)

Committer:
Soyoon
Date:
Mon Aug 01 02:23:11 2016 +0000
Revision:
3:880d00d555c1
Parent:
2:391e8bf671ef 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soyoon 0:6ac6b2d2bf1a 1 #include "mbed.h"
Soyoon 0:6ac6b2d2bf1a 2 #include "Barometer.h"
Soyoon 1:cd11c1c592c7 3 #include "LocalFileSystem.h"
Soyoon 0:6ac6b2d2bf1a 4 #include "math.h"
Soyoon 2:391e8bf671ef 5 #include "Servo.h"
Soyoon 1:cd11c1c592c7 6
Soyoon 0:6ac6b2d2bf1a 7 Serial pc(USBTX, USBRX);
Soyoon 2:391e8bf671ef 8 Timer end;
Soyoon 1:cd11c1c592c7 9 int stat=1;
Soyoon 0:6ac6b2d2bf1a 10
Soyoon 1:cd11c1c592c7 11 ////////////////////////////////////
Soyoon 2:391e8bf671ef 12 // GPS 5V p27(RX) //
Soyoon 1:cd11c1c592c7 13 // Bluetooth 3.3V p28(TX) //
Soyoon 1:cd11c1c592c7 14 ////////////////////////////////////
Soyoon 1:cd11c1c592c7 15 Serial Blue_GPS(p28, p27);
Soyoon 2:391e8bf671ef 16
Soyoon 3:880d00d555c1 17 char GPS_msg[2000], ns, ew;
Soyoon 2:391e8bf671ef 18 int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
Soyoon 0:6ac6b2d2bf1a 19 volatile unsigned char GPS_buffer[2];
Soyoon 1:cd11c1c592c7 20 float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
Soyoon 3:880d00d555c1 21 int blue_ok=0;
Soyoon 0:6ac6b2d2bf1a 22
Soyoon 0:6ac6b2d2bf1a 23 void GPS_isr(){
Soyoon 3:880d00d555c1 24 while(Blue_GPS.readable()){
Soyoon 3:880d00d555c1 25 i++;
Soyoon 3:880d00d555c1 26 GPS_buffer[1] = GPS_buffer[0];
Soyoon 3:880d00d555c1 27 GPS_buffer[0] = Blue_GPS.getc();
Soyoon 3:880d00d555c1 28 if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
Soyoon 3:880d00d555c1 29 i=0;
Soyoon 3:880d00d555c1 30 if (GPS_flag == 1){GPS_flag = 0; gps_ok = 1; j=0;}
Soyoon 3:880d00d555c1 31 }
Soyoon 3:880d00d555c1 32 if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
Soyoon 3:880d00d555c1 33 if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++; pc.printf("%d \r\n", j);}
Soyoon 0:6ac6b2d2bf1a 34 }
Soyoon 3:880d00d555c1 35
Soyoon 3:880d00d555c1 36 //Blue_GPS.printf("");
Soyoon 0:6ac6b2d2bf1a 37 }
Soyoon 0:6ac6b2d2bf1a 38
Soyoon 0:6ac6b2d2bf1a 39 float trunc(float v) {
Soyoon 0:6ac6b2d2bf1a 40 if(v < 0.0) {
Soyoon 0:6ac6b2d2bf1a 41 v*= -1.0;
Soyoon 0:6ac6b2d2bf1a 42 v = floor(v);
Soyoon 0:6ac6b2d2bf1a 43 v*=-1.0;
Soyoon 1:cd11c1c592c7 44 }
Soyoon 1:cd11c1c592c7 45 else {v = floor(v);}
Soyoon 0:6ac6b2d2bf1a 46 return v;
Soyoon 0:6ac6b2d2bf1a 47 }
Soyoon 0:6ac6b2d2bf1a 48
Soyoon 1:cd11c1c592c7 49 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)
Soyoon 1:cd11c1c592c7 50 {
Soyoon 1:cd11c1c592c7 51 if (gps_ok == 1){
Soyoon 1:cd11c1c592c7 52 gps_ok = 0;
Soyoon 2:391e8bf671ef 53 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);
Soyoon 1:cd11c1c592c7 54 if(*ns == 'S') {*latitude *= -1.0; }
Soyoon 1:cd11c1c592c7 55 if(*ew == 'W') {*longitude *= -1.0; }
Soyoon 1:cd11c1c592c7 56 float degrees = trunc(*latitude / 100.0f);
Soyoon 1:cd11c1c592c7 57 float minutes = (*latitude - (degrees * 100.0f));
Soyoon 1:cd11c1c592c7 58 *latitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 59 degrees = trunc(*longitude / 100.0f);
Soyoon 1:cd11c1c592c7 60 minutes = *longitude - (degrees * 100.0f);
Soyoon 1:cd11c1c592c7 61 *longitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 62 *Kor_time = *Kor_time + 90000;
Soyoon 1:cd11c1c592c7 63 }
Soyoon 1:cd11c1c592c7 64 }
Soyoon 1:cd11c1c592c7 65
Soyoon 2:391e8bf671ef 66 //Bluetooth code is placed under the Log_data
Soyoon 2:391e8bf671ef 67
Soyoon 2:391e8bf671ef 68 ////////////////////////////////////////////
Soyoon 2:391e8bf671ef 69 // Barometer 3.3V p9(SDA) p10(SCL) //
Soyoon 2:391e8bf671ef 70 ////////////////////////////////////////////
Soyoon 2:391e8bf671ef 71 Barometer barometer(p9, p10);
Soyoon 2:391e8bf671ef 72 float t, alt, del_alt;
Soyoon 2:391e8bf671ef 73 float alt_sum=0.0f, alt_zero=0.0f;
Soyoon 2:391e8bf671ef 74 int count = 0, baro_ok = 0; // for zero-calibration
Soyoon 2:391e8bf671ef 75 float alt_buffer[4], w_alt=0.6; // weight for LPF
Soyoon 2:391e8bf671ef 76
Soyoon 3:880d00d555c1 77 void get_Baro(float*t, float*alt, float*del_alt)
Soyoon 2:391e8bf671ef 78 {
Soyoon 2:391e8bf671ef 79 if (baro_ok==1){
Soyoon 2:391e8bf671ef 80 barometer.update();
Soyoon 2:391e8bf671ef 81 *t = barometer.get_temperature();
Soyoon 2:391e8bf671ef 82 *alt = barometer.get_altitude_m();
Soyoon 3:880d00d555c1 83 alt_buffer[1] = alt_buffer[0];
Soyoon 3:880d00d555c1 84 alt_buffer[0] = *alt;
Soyoon 3:880d00d555c1 85 *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter
Soyoon 3:880d00d555c1 86 alt_buffer[3] = alt_buffer[2];
Soyoon 3:880d00d555c1 87 alt_buffer[2] = *alt;
Soyoon 3:880d00d555c1 88 *del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed
Soyoon 2:391e8bf671ef 89 baro_ok = 0;
Soyoon 2:391e8bf671ef 90 }
Soyoon 2:391e8bf671ef 91 }
Soyoon 2:391e8bf671ef 92
Soyoon 2:391e8bf671ef 93 void calb_alt(){
Soyoon 2:391e8bf671ef 94 if (alt==0){}
Soyoon 2:391e8bf671ef 95 else {
Soyoon 2:391e8bf671ef 96 if (count<=19){alt_sum = alt_sum + alt; count++;}
Soyoon 2:391e8bf671ef 97 else {
Soyoon 2:391e8bf671ef 98 Blue_GPS.printf("calibration\r\n");
Soyoon 2:391e8bf671ef 99 alt_zero = alt_sum / (float)count;
Soyoon 2:391e8bf671ef 100 count++;
Soyoon 2:391e8bf671ef 101 }
Soyoon 2:391e8bf671ef 102 }
Soyoon 2:391e8bf671ef 103 }
Soyoon 3:880d00d555c1 104 /*
Soyoon 2:391e8bf671ef 105 void lpf_alt(){
Soyoon 2:391e8bf671ef 106 alt_buffer[1] = alt_buffer[0];
Soyoon 2:391e8bf671ef 107 alt_buffer[0] = alt;
Soyoon 2:391e8bf671ef 108 alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 109 alt_buffer[3] = alt_buffer[2];
Soyoon 2:391e8bf671ef 110 alt_buffer[2] = alt;
Soyoon 2:391e8bf671ef 111 del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed
Soyoon 2:391e8bf671ef 112 }
Soyoon 3:880d00d555c1 113 */
Soyoon 1:cd11c1c592c7 114 ///////////////////////////////////////
Soyoon 2:391e8bf671ef 115 // AHRS 5V p14(RX) // 20Hz
Soyoon 1:cd11c1c592c7 116 ///////////////////////////////////////
Soyoon 1:cd11c1c592c7 117 Serial AHRS(p13, p14);
Soyoon 1:cd11c1c592c7 118 float roll,pitch,yaw,accx,accy,accz;
Soyoon 2:391e8bf671ef 119 char AHRS_msg[150];
Soyoon 2:391e8bf671ef 120 int m=0, ahrs_ok=0, AHRS_flag;
Soyoon 2:391e8bf671ef 121 volatile unsigned char AHRS_buffer[2];
Soyoon 2:391e8bf671ef 122 float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
Soyoon 2:391e8bf671ef 123
Soyoon 2:391e8bf671ef 124 void AHRS_isr(){ //inturupt
Soyoon 2:391e8bf671ef 125 while(AHRS.readable()){
Soyoon 2:391e8bf671ef 126 AHRS_buffer[1] = AHRS_buffer[0];
Soyoon 2:391e8bf671ef 127 AHRS_buffer[0] = AHRS.getc();
Soyoon 2:391e8bf671ef 128 if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
Soyoon 2:391e8bf671ef 129 if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
Soyoon 2:391e8bf671ef 130 if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
Soyoon 2:391e8bf671ef 131 }
Soyoon 2:391e8bf671ef 132 }
Soyoon 1:cd11c1c592c7 133
Soyoon 1:cd11c1c592c7 134 void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
Soyoon 1:cd11c1c592c7 135 {
Soyoon 2:391e8bf671ef 136 if (ahrs_ok == 1){
Soyoon 2:391e8bf671ef 137 ahrs_ok = 0;
Soyoon 2:391e8bf671ef 138 sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
Soyoon 3:880d00d555c1 139 roll_buffer[1] = roll_buffer[0];
Soyoon 3:880d00d555c1 140 roll_buffer[0] = *roll;
Soyoon 3:880d00d555c1 141 *roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter
Soyoon 3:880d00d555c1 142 pitch_buffer[1] = pitch_buffer[0];
Soyoon 3:880d00d555c1 143 pitch_buffer[0] = *pitch;
Soyoon 3:880d00d555c1 144 *pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter
Soyoon 3:880d00d555c1 145 yaw_buffer[1] = yaw_buffer[0];
Soyoon 3:880d00d555c1 146 yaw_buffer[0] = *yaw;
Soyoon 3:880d00d555c1 147 *yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 148 baro_ok = 1;
Soyoon 3:880d00d555c1 149 blue_ok = 1;
Soyoon 2:391e8bf671ef 150 }
Soyoon 3:880d00d555c1 151 }
Soyoon 3:880d00d555c1 152 /*
Soyoon 2:391e8bf671ef 153 void lpf_attitude(){
Soyoon 2:391e8bf671ef 154 roll_buffer[1] = roll_buffer[0];
Soyoon 2:391e8bf671ef 155 roll_buffer[0] = roll;
Soyoon 2:391e8bf671ef 156 roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 157 pitch_buffer[1] = pitch_buffer[0];
Soyoon 2:391e8bf671ef 158 pitch_buffer[0] = pitch;
Soyoon 2:391e8bf671ef 159 pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 160 yaw_buffer[1] = yaw_buffer[0];
Soyoon 2:391e8bf671ef 161 yaw_buffer[0] = yaw;
Soyoon 2:391e8bf671ef 162 yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter
Soyoon 1:cd11c1c592c7 163 }
Soyoon 3:880d00d555c1 164 */
Soyoon 1:cd11c1c592c7 165
Soyoon 1:cd11c1c592c7 166 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 167 // Servo 5V PWM //
Soyoon 1:cd11c1c592c7 168 ///////////////////////////////////
Soyoon 2:391e8bf671ef 169 Servo Micro_gf(p22);
Soyoon 1:cd11c1c592c7 170
Soyoon 2:391e8bf671ef 171 float unf_value=1.0, gf_value = 0.35,linear_value = 1.0;
Soyoon 2:391e8bf671ef 172 float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
Soyoon 2:391e8bf671ef 173 void ctl_gf(){
Soyoon 2:391e8bf671ef 174 err_yaw = yaw - tg_yaw;
Soyoon 2:391e8bf671ef 175 gf_value = 0.35*exp(0.0039*p*err_yaw);
Soyoon 2:391e8bf671ef 176 if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
Soyoon 2:391e8bf671ef 177 if (gf_value<=0.0){gf_value=0.0;}
Soyoon 2:391e8bf671ef 178 else if (gf_value>=0.7) {gf_value=0.7;}
Soyoon 2:391e8bf671ef 179 Micro_gf = gf_value;
Soyoon 1:cd11c1c592c7 180 }
Soyoon 1:cd11c1c592c7 181
Soyoon 1:cd11c1c592c7 182 ///////////////////////////////
Soyoon 1:cd11c1c592c7 183 // Datalogger Mbed //
Soyoon 1:cd11c1c592c7 184 ///////////////////////////////
Soyoon 1:cd11c1c592c7 185 LocalFileSystem local("local");
Soyoon 2:391e8bf671ef 186 int file_no=1;
Soyoon 2:391e8bf671ef 187 char filename[256];
Soyoon 1:cd11c1c592c7 188 FILE *fp;
Soyoon 1:cd11c1c592c7 189
Soyoon 2:391e8bf671ef 190 void Log_file(){
Soyoon 2:391e8bf671ef 191 sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
Soyoon 2:391e8bf671ef 192 fp = fopen(filename, "r"); // if file can be loaded, return is 1
Soyoon 2:391e8bf671ef 193 while(fp){
Soyoon 2:391e8bf671ef 194 fclose(fp);
Soyoon 2:391e8bf671ef 195 file_no ++;
Soyoon 2:391e8bf671ef 196 sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing
Soyoon 2:391e8bf671ef 197 fp = fopen(filename, "r");
Soyoon 2:391e8bf671ef 198 }
Soyoon 2:391e8bf671ef 199 fp = fopen(filename, "w");
Soyoon 2:391e8bf671ef 200 }
Soyoon 2:391e8bf671ef 201
Soyoon 1:cd11c1c592c7 202 void Log_data(){
Soyoon 2:391e8bf671ef 203 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);
Soyoon 1:cd11c1c592c7 204 }
Soyoon 1:cd11c1c592c7 205
Soyoon 2:391e8bf671ef 206 void send_Blue(){
Soyoon 3:880d00d555c1 207 if (blue_ok == 1){
Soyoon 3:880d00d555c1 208 blue_ok = 0;
Soyoon 3:880d00d555c1 209 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);
Soyoon 3:880d00d555c1 210 }
Soyoon 3:880d00d555c1 211 }
Soyoon 3:880d00d555c1 212 void send_PC(){
Soyoon 3:880d00d555c1 213 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);
Soyoon 2:391e8bf671ef 214 }
Soyoon 2:391e8bf671ef 215
Soyoon 3:880d00d555c1 216 Ticker timer1;
Soyoon 3:880d00d555c1 217 Ticker timer2;
Soyoon 3:880d00d555c1 218 bool gpsFlag = 0;
Soyoon 3:880d00d555c1 219 bool blueFlag = 0;
Soyoon 3:880d00d555c1 220 void timer1_isr(){
Soyoon 3:880d00d555c1 221 gpsFlag = 1;
Soyoon 3:880d00d555c1 222 }
Soyoon 3:880d00d555c1 223 void timer2_isr(){
Soyoon 3:880d00d555c1 224 blueFlag = 1;
Soyoon 3:880d00d555c1 225 }
Soyoon 2:391e8bf671ef 226 /////////////////////////////////
Soyoon 2:391e8bf671ef 227 // Main loop //
Soyoon 2:391e8bf671ef 228 /////////////////////////////////
Soyoon 2:391e8bf671ef 229
Soyoon 0:6ac6b2d2bf1a 230 int main(void)
Soyoon 1:cd11c1c592c7 231 {
Soyoon 1:cd11c1c592c7 232 AHRS.baud(9600);
Soyoon 1:cd11c1c592c7 233 Blue_GPS.baud(9600);
Soyoon 3:880d00d555c1 234
Soyoon 3:880d00d555c1 235 while(Blue_GPS.readable()){
Soyoon 3:880d00d555c1 236 volatile unsigned char temp = Blue_GPS.getc();
Soyoon 3:880d00d555c1 237 }
Soyoon 3:880d00d555c1 238 Blue_GPS.printf("Buffer flushing\r\n");
Soyoon 3:880d00d555c1 239
Soyoon 1:cd11c1c592c7 240 Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
Soyoon 2:391e8bf671ef 241 AHRS.attach(&AHRS_isr, Serial::RxIrq);
Soyoon 2:391e8bf671ef 242 Blue_GPS.printf("Start\r\n");
Soyoon 2:391e8bf671ef 243 //Log_file();
Soyoon 3:880d00d555c1 244 timer1.attach(&timer1_isr, 1.0);
Soyoon 3:880d00d555c1 245 timer2.attach(&timer2_isr, 0.1);
Soyoon 2:391e8bf671ef 246 gf_value = 0.7;
Soyoon 2:391e8bf671ef 247 Micro_gf = gf_value;
Soyoon 1:cd11c1c592c7 248 while(1) {
Soyoon 1:cd11c1c592c7 249 switch(stat){
Soyoon 2:391e8bf671ef 250 case 1 : //Calibration
Soyoon 3:880d00d555c1 251 if(gpsFlag)
Soyoon 3:880d00d555c1 252 {
Soyoon 3:880d00d555c1 253 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 3:880d00d555c1 254 Blue_GPS.printf("getGPS\r\n");
Soyoon 3:880d00d555c1 255 gpsFlag = 0;
Soyoon 3:880d00d555c1 256 }
Soyoon 3:880d00d555c1 257 if(blueFlag)
Soyoon 3:880d00d555c1 258 {
Soyoon 3:880d00d555c1 259 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 3:880d00d555c1 260 get_Baro(&t, &alt, &del_alt);
Soyoon 3:880d00d555c1 261 calb_alt();
Soyoon 3:880d00d555c1 262 Blue_GPS.printf("%i,%.2f,%.2f,%.2f\r\n",count, alt, alt_sum, alt_zero);
Soyoon 3:880d00d555c1 263 blueFlag = 0;
Soyoon 3:880d00d555c1 264 }
Soyoon 2:391e8bf671ef 265 if (20<count) {
Soyoon 2:391e8bf671ef 266 stat=2;
Soyoon 2:391e8bf671ef 267 }
Soyoon 1:cd11c1c592c7 268 break;
Soyoon 2:391e8bf671ef 269 case 2 : //Wait
Soyoon 3:880d00d555c1 270 if(gpsFlag)
Soyoon 3:880d00d555c1 271 {
Soyoon 3:880d00d555c1 272 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 3:880d00d555c1 273 gpsFlag = 0;
Soyoon 3:880d00d555c1 274 }
Soyoon 3:880d00d555c1 275
Soyoon 3:880d00d555c1 276 if(blueFlag)
Soyoon 3:880d00d555c1 277 {
Soyoon 3:880d00d555c1 278 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 3:880d00d555c1 279 get_Baro(&t, &alt, &del_alt);
Soyoon 3:880d00d555c1 280 //Log_data();
Soyoon 3:880d00d555c1 281 alt = alt - alt_zero;
Soyoon 3:880d00d555c1 282 send_Blue();
Soyoon 3:880d00d555c1 283 send_PC();
Soyoon 3:880d00d555c1 284 blueFlag = 0;
Soyoon 3:880d00d555c1 285 }
Soyoon 2:391e8bf671ef 286 if (alt<=-10 && abs(accx)<0.1 && abs(accy)<0.1){
Soyoon 2:391e8bf671ef 287 //fclose(fp);
Soyoon 2:391e8bf671ef 288 stat=3; send_Blue();
Soyoon 2:391e8bf671ef 289 }
Soyoon 2:391e8bf671ef 290 break;
Soyoon 2:391e8bf671ef 291 /*case 3 : //Drop
Soyoon 1:cd11c1c592c7 292 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 293 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 294 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 295 //Log_data();
Soyoon 2:391e8bf671ef 296 lpf_alt();
Soyoon 1:cd11c1c592c7 297 alt = alt - alt_zero;
Soyoon 2:391e8bf671ef 298 lpf_attitude();
Soyoon 2:391e8bf671ef 299 send_Blue();
Soyoon 2:391e8bf671ef 300 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);
Soyoon 2:391e8bf671ef 301 if (alt<=5 && abs(accx)<0.1 && abs(accy)<0.1) {end.start(); stat=4;}
Soyoon 1:cd11c1c592c7 302 break;
Soyoon 2:391e8bf671ef 303 case 4 : //Landing
Soyoon 1:cd11c1c592c7 304 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 305 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 306 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 2:391e8bf671ef 307 //Log_data();
Soyoon 2:391e8bf671ef 308 lpf_alt();
Soyoon 1:cd11c1c592c7 309 alt = alt - alt_zero;
Soyoon 2:391e8bf671ef 310 lpf_attitude();
Soyoon 2:391e8bf671ef 311 send_Blue();
Soyoon 2:391e8bf671ef 312 end.read();
Soyoon 2:391e8bf671ef 313 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);
Soyoon 2:391e8bf671ef 314 if (end.read()>=10) {fclose(fp);}
Soyoon 2:391e8bf671ef 315 break;*/
Soyoon 2:391e8bf671ef 316 case 3 : //Shut down
Soyoon 1:cd11c1c592c7 317 break;
Soyoon 0:6ac6b2d2bf1a 318 }
Soyoon 1:cd11c1c592c7 319 }
Soyoon 1:cd11c1c592c7 320 }