Fork of
mbed_main
by 
CANSAT_AIRFUL
Diff: main.cpp
- Revision:
- 3:e3e965924dde
- Parent:
- 2:9d0f979369cf
- Child:
- 4:4324f20e4597
--- a/main.cpp Wed Jul 27 16:17:26 2016 +0000
+++ b/main.cpp Tue Aug 02 14:38:15 2016 +0000
@@ -4,7 +4,7 @@
#include "math.h"
#include "Servo.h"
-Serial pc(USBTX, USBRX);
+Ticker blue_trig;
Timer end;
int stat=1;
@@ -17,6 +17,7 @@
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++;
@@ -57,6 +58,21 @@
}
}
+void blue_trig_isr(){
+ blue_ok=1; // AHRS 20HZ
+}
+
+void trans_blue_data(float in_data, int integer_point, int under_point){ // number of intefer and under_point
+ unsigned int conv_trans_data;
+ conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
+ if(in_data<0) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '-';}
+ else {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '+';}
+ for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
+ if(cnt_num == under_point) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '.'; }
+ 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
+ }
+}
+
//Bluetooth code is placed under the Log_data
////////////////////////////////////////////
@@ -66,48 +82,54 @@
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
+float alt_buffer[4], w_alt=0.6; // weight for LPF
-void get_Baro(float*t, float*alt)
+void get_Baro(float*alt, float*del_alt)
{
if (baro_ok==1){
- barometer.update();
- *t = barometer.get_temperature();
- *alt = barometer.get_altitude_m();
- baro_ok = 0;
+ barometer.update();
+ *alt = barometer.get_altitude_m();
+ alt_buffer[1] = alt_buffer[0];
+ alt_buffer[0] = *alt;
+ if(abs(alt_buffer[0]- alt_buffer[1])>100){
+ alt_buffer[0] = alt_buffer[1];
+ *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter
+ *del_alt = 0; // for calculation of drop speed
+ baro_ok = 0;
+ }
+ else{
+ *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;
+ }
}
}
void calb_alt(){
- if (alt==0){}
+ if (alt==0){count=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++;
+ if (count==1){count++;}
+ else{
+ if (count<=99){alt_sum = alt_sum + alt; count++;}
+ else {
+ alt_zero = alt_sum / (float)(count-2);
+ 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) // 20Hz
///////////////////////////////////////
Serial AHRS(p13, p14);
float roll,pitch,yaw,accx,accy,accz;
char AHRS_msg[150];
-int m=0, ahrs_ok=0, AHRS_flag;
+int m=0, ahrs_ok=0, AHRS_flag=0;
volatile unsigned char AHRS_buffer[2];
-float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
+float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.7;
void AHRS_isr(){ //inturupt
while(AHRS.readable()){
@@ -124,21 +146,18 @@
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;
}
-}
-
-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
-}
+}
///////////////////////////////////
// Servo 5V PWM //
@@ -147,7 +166,7 @@
Servo Micro_gf(p22);
Servo Linear(p23);
-float unf_value = 1.0, gf_value = 0.35, linear_value = 0.5;
+float unf_value=1.0, gf_value = 0.35, linear_value = 0.25;
float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
void ctl_gf(){
@@ -159,39 +178,6 @@
Micro_gf = gf_value;
}
-///////////////////////////////////
-// Checksum //
-///////////////////////////////////
-long chksum;
-
-unsigned int checksum (int data, short message_length)
-{
- unsigned int sum = 10;
- /*for (i=0;i<=message_length; i++){
- sum += data[i];
- }
- sum = (sum & 0xff);*/
- return (unsigned int)(~sum);
-}
-/*
-float data[15];
-void packet_data(){
- data[0] = stat;
- data[1] = Kor_time;
- 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 //
///////////////////////////////
@@ -213,49 +199,77 @@
}
void Log_data(){
- 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);
+ 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);
}
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);
+ if (blue_ok == 1){
+ blue_ok = 0;
+ trans_blue_data((float)stat,1,0);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; // basic formation of way to send data
+ trans_blue_data(Kor_time,6,0);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(latitude,2,6);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(longitude,3,6);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(alt,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(del_alt,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(alt_GPS,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(roll,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(pitch,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(yaw,3,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(accz,2,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(unf_value,1,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(gf_value,1,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ trans_blue_data(linear_value,1,2);
+ while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '\n';
+ }
}
+
/////////////////////////////////
-// Main loop //
+// Main loop //
/////////////////////////////////
int main(void)
{
AHRS.baud(9600);
Blue_GPS.baud(9600);
- Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
- AHRS.attach(&AHRS_isr, Serial::RxIrq);
- Micro_unf = unf_value; //Initial setting of servo
- pc.printf("Start\r\n");
+ Blue_GPS.attach(&GPS_isr);
+ AHRS.attach(&AHRS_isr);
+ blue_trig.attach(&blue_trig_isr, 0.125);
+ Micro_gf = gf_value; //Initial setting of servo
while(1) {
switch(stat){
case 1 : //Bluetooth connection and 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);
+ get_Baro(&alt, &del_alt);
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;}
+ if (100<count) {stat=2;}
break;
- case 2 : //Waiting launch
+ 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
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);
+ get_Baro(&alt, &del_alt);
alt = alt - alt_zero;
send_Blue();
if (100.0<alt_GPS && del_alt>1){
stat=3;
Log_file();
- unf_value=0.0;
Micro_unf = unf_value;
+ unf_value = 0.0; //checking
wait(0.5);
Micro_gf = gf_value;
}
@@ -263,36 +277,44 @@
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);
+ get_Baro(&alt, &del_alt);
Log_data();
- lpf_alt();
alt = alt - alt_zero;
- lpf_attitude();
send_Blue();
- if (alt<10){stat=4; Linear = linear_value;} // need to modify trigger
+ if (alt<10){ // need to modify trigger
+ fclose(fp);
+ Log_file();
+ stat=4;
+ Linear=linear_value;
+ linear_value=0.0;
+ }
break;
case 4 : //Reverse thrust for soft landing
+ 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);
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ get_Baro(&alt, &del_alt);
Log_data();
- lpf_alt();
alt = alt - alt_zero;
- lpf_attitude();
send_Blue();
- if (accx<0.1 && accy<0.1 && alt<3) {end.start(); stat=5;}
+ if (accx<0.1 && accy<0.1 && alt<3){
+ fclose(fp);
+ Log_file();
+ end.start();
+ stat=5;
+ }
break;
case 5 : //Landing
+ 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);
- get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+ get_Baro(&alt, &del_alt);
Log_data();
- lpf_alt();
alt = alt - alt_zero;
- lpf_attitude();
send_Blue();
end.read();
- if (end.read()>=10) {fclose(fp); stat=6;}
+ if (end.read()>=10) {
+ fclose(fp);
+ stat=6;
+ }
break;
case 6 : //Shut down
break;