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
Diff: main.cpp
- Revision:
- 3:880d00d555c1
- Parent:
- 2:391e8bf671ef
- Child:
- 4:62aec4ae9f67
--- a/main.cpp Sat Jul 30 16:42:10 2016 +0000
+++ b/main.cpp Mon Aug 01 02:23:11 2016 +0000
@@ -14,21 +14,26 @@
////////////////////////////////////
Serial Blue_GPS(p28, p27);
-char GPS_msg[150], ns, ew;
+char GPS_msg[2000], ns, ew;
int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
volatile unsigned char GPS_buffer[2];
float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
+int blue_ok=0;
void GPS_isr(){
- i++;
- GPS_buffer[1] = GPS_buffer[0];
- GPS_buffer[0] = Blue_GPS.getc();
- if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
- i=0;
- if (GPS_flag == 1){GPS_flag = 0;gps_ok = 1;j=0;}
+ while(Blue_GPS.readable()){
+ i++;
+ GPS_buffer[1] = GPS_buffer[0];
+ GPS_buffer[0] = Blue_GPS.getc();
+ if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
+ i=0;
+ if (GPS_flag == 1){GPS_flag = 0; gps_ok = 1; j=0;}
+ }
+ if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
+ if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++; pc.printf("%d \r\n", j);}
}
- if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
- if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
+
+ //Blue_GPS.printf("");
}
float trunc(float v) {
@@ -69,12 +74,18 @@
int count = 0, baro_ok = 0; // for zero-calibration
float alt_buffer[4], w_alt=0.6; // weight for LPF
-void get_Baro(float*t, float*alt)
+void get_Baro(float*t, float*alt, float*del_alt)
{
if (baro_ok==1){
barometer.update();
*t = barometer.get_temperature();
*alt = barometer.get_altitude_m();
+ alt_buffer[1] = alt_buffer[0];
+ alt_buffer[0] = *alt;
+ *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter
+ alt_buffer[3] = alt_buffer[2];
+ alt_buffer[2] = *alt;
+ *del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed
baro_ok = 0;
}
}
@@ -90,7 +101,7 @@
}
}
}
-
+/*
void lpf_alt(){
alt_buffer[1] = alt_buffer[0];
alt_buffer[0] = alt;
@@ -99,7 +110,7 @@
alt_buffer[2] = alt;
del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed
}
-
+*/
///////////////////////////////////////
// AHRS 5V p14(RX) // 20Hz
///////////////////////////////////////
@@ -125,10 +136,20 @@
if (ahrs_ok == 1){
ahrs_ok = 0;
sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
+ roll_buffer[1] = roll_buffer[0];
+ roll_buffer[0] = *roll;
+ *roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter
+ pitch_buffer[1] = pitch_buffer[0];
+ pitch_buffer[0] = *pitch;
+ *pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter
+ yaw_buffer[1] = yaw_buffer[0];
+ yaw_buffer[0] = *yaw;
+ *yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter
baro_ok = 1;
+ blue_ok = 1;
}
-}
-
+}
+/*
void lpf_attitude(){
roll_buffer[1] = roll_buffer[0];
roll_buffer[0] = roll;
@@ -140,7 +161,7 @@
yaw_buffer[0] = yaw;
yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter
}
-
+*/
///////////////////////////////////
// Servo 5V PWM //
@@ -183,9 +204,25 @@
}
void send_Blue(){
- Blue_GPS.printf("%i,%.0f,%.3f,%.3f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+ if (blue_ok == 1){
+ blue_ok = 0;
+ 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);
+ }
+}
+void send_PC(){
+ 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);
}
+Ticker timer1;
+Ticker timer2;
+bool gpsFlag = 0;
+bool blueFlag = 0;
+void timer1_isr(){
+ gpsFlag = 1;
+}
+void timer2_isr(){
+ blueFlag = 1;
+}
/////////////////////////////////
// Main loop //
/////////////////////////////////
@@ -194,34 +231,58 @@
{
AHRS.baud(9600);
Blue_GPS.baud(9600);
+
+ while(Blue_GPS.readable()){
+ volatile unsigned char temp = Blue_GPS.getc();
+ }
+ Blue_GPS.printf("Buffer flushing\r\n");
+
Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
AHRS.attach(&AHRS_isr, Serial::RxIrq);
Blue_GPS.printf("Start\r\n");
//Log_file();
+ timer1.attach(&timer1_isr, 1.0);
+ timer2.attach(&timer2_isr, 0.1);
gf_value = 0.7;
Micro_gf = gf_value;
while(1) {
switch(stat){
case 1 : //Calibration
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
- get_Baro(&t, &alt);
- calb_alt();
- lpf_attitude();
- Blue_GPS.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero);
+ if(gpsFlag)
+ {
+ get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ Blue_GPS.printf("getGPS\r\n");
+ gpsFlag = 0;
+ }
+ if(blueFlag)
+ {
+ get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+ get_Baro(&t, &alt, &del_alt);
+ calb_alt();
+ Blue_GPS.printf("%i,%.2f,%.2f,%.2f\r\n",count, alt, alt_sum, alt_zero);
+ blueFlag = 0;
+ }
if (20<count) {
stat=2;
}
break;
case 2 : //Wait
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
- get_Baro(&t, &alt);
- //Log_data();
- alt = alt - alt_zero;
- lpf_alt();
- lpf_attitude();
- send_Blue();
+ if(gpsFlag)
+ {
+ get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ gpsFlag = 0;
+ }
+
+ if(blueFlag)
+ {
+ get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+ get_Baro(&t, &alt, &del_alt);
+ //Log_data();
+ alt = alt - alt_zero;
+ send_Blue();
+ send_PC();
+ blueFlag = 0;
+ }
if (alt<=-10 && abs(accx)<0.1 && abs(accy)<0.1){
//fclose(fp);
stat=3; send_Blue();