CANSAT_AIRFUL / Mbed 2 deprecated mbed_paper_mixing

mixing control

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@2:9d0f979369cf, 2016-07-27 (annotated)

Committer:
Soyoon
Date:
Wed Jul 27 16:17:26 2016 +0000
Revision:
2:9d0f979369cf
Parent:
1:cd11c1c592c7
Child:
3:e3e965924dde
Airful;

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