CANSAT_AIRFUL / Mbed 2 deprecated mbed_main

Main code

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@3:e3e965924dde, 2016-08-02 (annotated)

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