Kim Youngsik / Mbed 2 deprecated 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.

Dependencies:   Servo mbed

Fork of mbed_droptest by Soyoon Kim

main.cpp@4:62aec4ae9f67, 2016-08-01 (annotated)

Committer:
skyyoungsik
Date:
Mon Aug 01 09:29:24 2016 +0000
Revision:
4:62aec4ae9f67
Parent:
3:880d00d555c1 
resistry

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