CANSAT_AIRFUL
mixing control
Fork of
mbed_main
by 
CANSAT_AIRFUL
Diff: main.cpp
- Revision:
- 2:9d0f979369cf
- Parent:
- 1:cd11c1c592c7
- Child:
- 3:e3e965924dde
--- a/main.cpp Tue Jul 19 14:30:51 2016 +0000
+++ b/main.cpp Wed Jul 27 16:17:26 2016 +0000
@@ -1,20 +1,20 @@
#include "mbed.h"
#include "Barometer.h"
-#include "Ultrasonic.h"
#include "LocalFileSystem.h"
#include "math.h"
+#include "Servo.h"
Serial pc(USBTX, USBRX);
-int data[14];
+Timer end;
int stat=1;
////////////////////////////////////
-// GPS 5V p27(RX) //
+// GPS 5V p27(RX) //
// Bluetooth 3.3V p28(TX) //
////////////////////////////////////
Serial Blue_GPS(p28, p27);
-char msg[150], ns, ew;
-int i = 0, j=0, gps_ok=0, flag;
+char GPS_msg[150], 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;
@@ -24,10 +24,10 @@
GPS_buffer[0] = Blue_GPS.getc();
if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
i=0;
- if (flag == 1){flag = 0;gps_ok = 1;j=0;}
+ if (GPS_flag == 1){GPS_flag = 0;gps_ok = 1;j=0;}
}
- if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
- if (flag==1){msg[j]=GPS_buffer[0]; j++;}
+ if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
+ if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
}
float trunc(float v) {
@@ -44,7 +44,7 @@
{
if (gps_ok == 1){
gps_ok = 0;
- sscanf(msg, "GA,%f,%f,%c,%f,%c,%f,%f,%f,%f,%f", Kor_time,latitude,ns,longitude,ew,fix,sat,x,alt_GPS,lock);
+ 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);
if(*ns == 'S') {*latitude *= -1.0; }
if(*ew == 'W') {*longitude *= -1.0; }
float degrees = trunc(*latitude / 100.0f);
@@ -57,64 +57,113 @@
}
}
+//Bluetooth code is placed under the Log_data
+
+////////////////////////////////////////////
+// Barometer 3.3V p9(SDA) p10(SCL) //
+////////////////////////////////////////////
+Barometer barometer(p9, p10);
+float t, alt, del_alt;
+float alt_sum=0.0f, alt_zero=0.0f;
+int count = 0, baro_ok = 0; // for zero-calibration
+float alt_buffer[4], w_alt=0.4; // weight for LPF
+
+void get_Baro(float*t, float*alt)
+{
+ if (baro_ok==1){
+ barometer.update();
+ *t = barometer.get_temperature();
+ *alt = barometer.get_altitude_m();
+ baro_ok = 0;
+ }
+}
+
+void calb_alt(){
+ if (alt==0){}
+ else {
+ if (count<=99){alt_sum = alt_sum + alt; count++;}
+ else {
+ Blue_GPS.printf("calibration\r\n");
+ alt_zero = alt_sum / (float)count;
+ count++;
+ }
+ }
+}
+
+void lpf_alt(){
+ 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
+}
+
///////////////////////////////////////
-// AHRS 5V p14(RX) //
+// AHRS 5V p14(RX) // 20Hz
///////////////////////////////////////
Serial AHRS(p13, p14);
float roll,pitch,yaw,accx,accy,accz;
-float t, alt;
+char AHRS_msg[150];
+int m=0, ahrs_ok=0, AHRS_flag;
+volatile unsigned char AHRS_buffer[2];
+float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
+
+void AHRS_isr(){ //inturupt
+ while(AHRS.readable()){
+ AHRS_buffer[1] = AHRS_buffer[0];
+ AHRS_buffer[0] = AHRS.getc();
+ if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
+ if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
+ if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
+ }
+}
void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
{
- while(AHRS.getc() != '\n');
- AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", roll, pitch, yaw, accx, accy, accz);
-}
-
-////////////////////////////////////////////
-// Barometer 3.3V p9(SDA) p10(SCL) //
-////////////////////////////////////////////
-Barometer barometer(p9, p10);
-float alt_sum=0.0f, alt_zero=0.0f, alt_avg=0.0f; // for zero-calibration
-float alt_buffer[5];
-int count = 0;
-
-void get_Baro(float*t, float*alt)
-{
- barometer.update();
- *t = barometer.get_temperature();
- *alt = barometer.get_altitude_m();
-}
+ if (ahrs_ok == 1){
+ ahrs_ok = 0;
+ sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
+ baro_ok = 1;
+ }
+}
-void Baro_zero(){
- if (count<=4){
- alt_sum = alt_sum + alt;
- count++;
- }
- else {
- pc.printf("calibration\r\n");
- alt_zero = alt_sum / (float)count ;
- }
+void lpf_attitude(){
+ 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
}
-/*
-void Baro_check(){
- alt_buffer[0] = alt;
- for(i=0;i<=3;i++) {alt_buffer[i+1] = alt_buffer[i]; } //Save in 5 buffers
- del_alt = alt_min - alt_max;
-}
-*/
-
///////////////////////////////////
// Servo 5V PWM //
///////////////////////////////////
-PwmOut Linear(p21);
-PwmOut Micro_gf(p22);
-PwmOut Micro_unf(p23);
+Servo Micro_unf(p21);
+Servo Micro_gf(p22);
+Servo Linear(p23);
+
+float unf_value = 1.0, gf_value = 0.35, linear_value = 0.5;
+float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
+
+void ctl_gf(){
+ err_yaw = yaw - tg_yaw;
+ gf_value = 0.35*exp(0.0039*p*err_yaw);
+ if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
+ if (gf_value<=0.0){gf_value=0.0;}
+ else if (gf_value>=0.7) {gf_value=0.7;}
+ Micro_gf = gf_value;
+}
///////////////////////////////////
// Checksum //
///////////////////////////////////
long chksum;
+
unsigned int checksum (int data, short message_length)
{
unsigned int sum = 10;
@@ -124,47 +173,64 @@
sum = (sum & 0xff);*/
return (unsigned int)(~sum);
}
-
+/*
+float data[15];
void packet_data(){
data[0] = stat;
data[1] = Kor_time;
- data[2] = (int)(alt*100);
- data[3] = (int)(alt_GPS*100);
- data[4] = (int)(roll*100);
- data[5] = (int)(pitch*100);
- data[6] = (int)(yaw*100);
- data[7] = (int)(accz*100);
- data[8] = (int)(latitude*100);
- data[9] = (int)(longitude*100);
- data[10] = Micro_unf*100;
- data[11] = Micro_gf*100;
- data[12] = Linear*100;
+ data[2] = alt;
+ data[3] = alt_GPS;
+ data[4] = roll;
+ data[5] = pitch;
+ data[6] = yaw;
+ data[7] = accz;
+ data[8] = latitude;
+ data[9] = longitude;
+ data[10] = unf_value;
+ data[11] = gf_value;
+ data[12] = linear_value;
data[13] = chksum;
}
-
+*/
///////////////////////////////
// Datalogger Mbed //
///////////////////////////////
LocalFileSystem local("local");
-int num=0;
+int file_no=1;
+char filename[256];
FILE *fp;
+void Log_file(){
+ sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
+ fp = fopen(filename, "r"); // if file can be loaded, return is 1
+ while(fp){
+ fclose(fp);
+ file_no ++;
+ sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing
+ fp = fopen(filename, "r");
+ }
+ fp = fopen(filename, "w");
+}
+
void Log_data(){
- FILE *fp = fopen("/local/data.txt", "w");
- for (num=0;num<=13;num++) {fprintf(fp, "%i ",data[num]);}
- num=0;
- fprintf(fp, "\r\n");
+ 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);
}
+void send_Blue(){
+ 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);
+}
+
+/////////////////////////////////
+// Main loop //
+/////////////////////////////////
+
int main(void)
{
- wait(10);
AHRS.baud(9600);
Blue_GPS.baud(9600);
Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
- Linear.period(0.01); Micro_gf.period(0.01); Micro_unf.period(0.01);
- Linear=0.19; Micro_unf=0.2; Micro_gf=0.2;//Initial setting of servo
- //Linear=0.1;wait(1);Take back
+ AHRS.attach(&AHRS_isr, Serial::RxIrq);
+ Micro_unf = unf_value; //Initial setting of servo
pc.printf("Start\r\n");
while(1) {
switch(stat){
@@ -172,44 +238,63 @@
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);
- Baro_zero();
- packet_data();
- //Log_data();
- pc.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero);
- //if (Kor_time!=0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
- if (5<=count){Baro_zero(); pc.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero); stat=2;}
+ calb_alt();
+ lpf_attitude();
+ send_Blue();
+ Blue_GPS.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero);
+ if (Kor_time!=0.0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
break;
case 2 : //Waiting launch
+ get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+ lpf_attitude();
get_Baro(&t, &alt);
alt = alt - alt_zero;
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- packet_data();
- pc.printf("%i %i %i %i %i %i %i %i %i %i %i %i %i %i %i %i\r\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11],data[12],data[13]);
- if (100.0<alt_GPS && accz<0){stat=3;}
+ send_Blue();
+ if (100.0<alt_GPS && del_alt>1){
+ stat=3;
+ Log_file();
+ unf_value=0.0;
+ Micro_unf = unf_value;
+ wait(0.5);
+ Micro_gf = gf_value;
+ }
break;
case 3 : //Grid fin and landing leg spreading and control
+ 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();
+ lpf_alt();
alt = alt - alt_zero;
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- packet_data();
- if (alt<3){stat=4;}
+ lpf_attitude();
+ send_Blue();
+ if (alt<10){stat=4; Linear = linear_value;} // need to modify trigger
break;
case 4 : //Reverse thrust for soft landing
get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
get_Baro(&t, &alt);
+ get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ Log_data();
+ lpf_alt();
alt = alt - alt_zero;
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- packet_data();
- if (accx<0.1 && accy<0.1 && accz<0.1) {stat=5;}
+ lpf_attitude();
+ send_Blue();
+ if (accx<0.1 && accy<0.1 && alt<3) {end.start(); stat=5;}
break;
case 5 : //Landing
get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
get_Baro(&t, &alt);
+ get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ Log_data();
+ lpf_alt();
alt = alt - alt_zero;
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
- packet_data();
+ lpf_attitude();
+ send_Blue();
+ end.read();
+ if (end.read()>=10) {fclose(fp); stat=6;}
+ break;
+ case 6 : //Shut down
break;
}
}