Soyoon Kim
/
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.
Revision 3:880d00d555c1, committed 2016-08-01
- Comitter:
- Soyoon
- Date:
- Mon Aug 01 02:23:11 2016 +0000
- Parent:
- 2:391e8bf671ef
- Commit message:
- test
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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();