CANSAT_AIRFUL
step3
Fork of
mbed_main
by 
CANSAT_AIRFUL
Revision 4:3ed1a684a383, committed 2016-11-19
- Comitter:
- Soyoon
- Date:
- Sat Nov 19 15:05:43 2016 +0000
- Parent:
- 3:e3e965924dde
- Commit message:
- 3; ;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Aug 02 14:38:15 2016 +0000
+++ b/main.cpp Sat Nov 19 15:05:43 2016 +0000
@@ -3,73 +3,52 @@
#include "LocalFileSystem.h"
#include "math.h"
#include "Servo.h"
+#define dt 0.1
+Serial pc(USBTX, USBRX);
Ticker blue_trig;
Timer end;
-int stat=1;
-////////////////////////////////////
-// GPS 5V p27(RX) //
-// Bluetooth 3.3V p28(TX) //
-////////////////////////////////////
-Serial Blue_GPS(p28, p27);
-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;
-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;}
- }
- if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
- if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
-}
+////////////////////////////////////////
+// Bluetooth 3.3V p13 p14(TX,RX) //
+////////////////////////////////////////
+Serial Blue(p13, p14);
+int send_ok=0;
+char Blue_msg[150];
+int k=0, Blue_ok=0, Blue_flag=0;
+volatile unsigned char Blue_buffer[2];
+float input_roll, input_pitch, input_yaw, input_thr;
-float trunc(float v) {
- if(v < 0.0) {
- v*= -1.0;
- v = floor(v);
- v*=-1.0;
+void Blue_isr(){ //inturupt
+ while(Blue.readable()){
+ Blue_buffer[1] = Blue_buffer[0];
+ Blue_buffer[0] = Blue.getc();
+ if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
+ if (Blue_buffer[0] == '*'){Blue_flag=1;}
+ if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
}
- else {v = floor(v);}
- return v;
-}
+}
-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)
+void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
{
- if (gps_ok == 1){
- gps_ok = 0;
- 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);
- float minutes = (*latitude - (degrees * 100.0f));
- *latitude = degrees + minutes / 60.0f;
- degrees = trunc(*longitude / 100.0f);
- minutes = *longitude - (degrees * 100.0f);
- *longitude = degrees + minutes / 60.0f;
- *Kor_time = *Kor_time + 90000;
+ if (Blue_ok == 1){
+ Blue_ok = 0;
+ sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
}
-}
+}
void blue_trig_isr(){
- blue_ok=1; // AHRS 20HZ
+ send_ok=1;
}
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 = '+';}
+ if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
+ else {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->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
+ if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.'; }
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48); //convert to ASCII
}
}
@@ -79,29 +58,23 @@
// Barometer 3.3V p9(SDA) p10(SCL) //
////////////////////////////////////////////
Barometer barometer(p9, p10);
-float t, alt, del_alt;
+float alt=0.0;
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.6; // weight for LPF
+float alt_buffer[2], w_alt=0; // weight for LPF
-void get_Baro(float*alt, float*del_alt)
+void get_Baro(float*alt)
{
if (baro_ok==1){
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
+ if(abs(alt_buffer[0]- alt_buffer[1])>20){
+ *alt = alt_buffer[1];
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;
}
}
@@ -113,23 +86,19 @@
if (count==1){count++;}
else{
if (count<=99){alt_sum = alt_sum + alt; count++;}
- else {
- alt_zero = alt_sum / (float)(count-2);
- count++;
- }
+ else {alt_zero = alt_sum/(float)(count-1); count++;}
}
}
}
///////////////////////////////////////
-// AHRS 5V p14(RX) // 20Hz
+// AHRS 5V p27(RX) // 20Hz
///////////////////////////////////////
-Serial AHRS(p13, p14);
-float roll,pitch,yaw,accx,accy,accz;
+Serial AHRS(p28, p27);
+float roll,pitch,yaw,velx,vely,velz,velxyz;
char AHRS_msg[150];
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.7;
void AHRS_isr(){ //inturupt
while(AHRS.readable()){
@@ -141,41 +110,89 @@
}
}
-void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
+void get_AHRS(float*roll, float*pitch, float*yaw, float*velx, float*vely, float*velz, float*velxyz)
{
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
+ sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
+ *velxyz = (float)sqrt(pow(*velx,2)+pow(*vely,2)+pow(*velz,2));
baro_ok = 1;
}
}
///////////////////////////////////
-// Servo 5V PWM //
+// Servo 5V PWM // needed to check pin#
///////////////////////////////////
-Servo Micro_unf(p21);
-Servo Micro_gf(p22);
-Servo Linear(p23);
+Servo Throttle(p26);
+Servo CS1(p25); //below
+Servo CS2(p23); //upper
+Servo CS3(p21); //below
+Servo CS4(p22); //upper
+
+float thr_value = 0.0, ctrl1_value = 0.5, ctrl2_value = 0.5, ctrl3_value = 0.5, ctrl4_value = 0.5;
+float err_roll = 0.0, err_pitch = 0.0, err_yaw = 0.0, err_alt = 0.0, preerror_roll = 0.0, preerror_pitch = 0.0, preerror_yaw = 0.0;
+float ctrl_roll = 0.0, ctrl_pitch = 0.0, ctrl_yaw = 0.0;
+float kp1=1.0, kd1=0.0, kp2=1.0, kd2=0.0, kp3=1.0, kd3=0.0; // PD Controller
-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 ctrl_attitude(){
+ err_roll = input_roll - roll;
+ err_pitch = input_pitch - pitch;
+ err_yaw = input_yaw - yaw;
+ ctrl_roll = ((kp1*err_roll)+(kd1*(err_roll-preerror_roll)/dt))/180.0;
+ ctrl_pitch = ((kp2*err_pitch)+(kd2*(err_pitch-preerror_pitch)/dt))/180.0;
+ ctrl_yaw = ((kp3*err_yaw)+(kd3*(err_yaw-preerror_yaw)/dt))/180.0; // PD control and Range adjustment
+ preerror_roll = err_roll;
+ preerror_pitch = err_pitch;
+ preerror_yaw = err_yaw;
+
+ // Roll = ctrl2 - ctrl4 Controll surface movement, Servo motor forwards same direction
+ // Pitch = ctrl1 - ctrl3
+ // Yaw = ctrl1 + ctrl2 + ctrl3 + ctrl4
+
+ //Servo1 Control (forward)
+ ctrl1_value = ((ctrl_pitch/2.0) + (ctrl_yaw/4.0));
+ if (ctrl1_value<0){ctrl1_value = 1.0-0.5*exp(0.0039*(-ctrl1_value));}
+ else {ctrl1_value = 0.5*exp(0.0039*ctrl1_value);}
+ if (ctrl1_value<=0.0){ctrl1_value=0.0;}
+ else if (ctrl1_value>=1.0) {ctrl1_value=1.0;}
+
+ //Servo2 Control (right)
+ ctrl2_value = ((ctrl_roll/2.0) + (ctrl_yaw/4.0));
+ if (ctrl2_value<0){ctrl2_value = 1.0-0.5*exp(0.0039*(-ctrl2_value));}
+ else {ctrl2_value = 0.5*exp(0.0039*ctrl2_value);}
+ if (ctrl2_value<=0.0){ctrl2_value=0.0;}
+ else if (ctrl2_value>=1.0) {ctrl2_value=1.0;}
+
+ //Servo3 Control (backward)
+ ctrl3_value = (-(ctrl_pitch/2.0) + (ctrl_yaw/4.0));
+ if (ctrl3_value<0){ctrl3_value = 1.0-0.5*exp(0.0039*(-ctrl3_value));}
+ else {ctrl3_value = 0.5*exp(0.0039*ctrl3_value);}
+ if (ctrl3_value<=0.0){ctrl3_value=0.0;}
+ else if (ctrl3_value>=1.0) {ctrl3_value=1.0;}
+
+ //Servo4 Control (left)
+ ctrl4_value = (-(ctrl_roll/2.0) + (ctrl_yaw/4.0));
+ if (ctrl4_value<0){ctrl4_value = 1.0-0.5*exp(0.0039*(-ctrl4_value));}
+ else {ctrl4_value = 0.5*exp(0.0039*ctrl4_value);}
+ if (ctrl4_value<=0.0){ctrl4_value=0.0;}
+ else if (ctrl4_value>=1.0) {ctrl4_value=1.0;}
+
+ CS1 = ctrl1_value;
+ CS2 = ctrl2_value;
+ CS3 = ctrl3_value;
+ CS4 = ctrl4_value;
+}
-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;
+
+void neutral(){
+ ctrl1_value = 0.5;
+ ctrl2_value = 0.5;
+ ctrl3_value = 0.5;
+ ctrl4_value = 0.5;
+ CS1 = ctrl1_value;
+ CS2 = ctrl2_value;
+ CS3 = ctrl3_value;
+ CS4 = ctrl4_value;
}
///////////////////////////////
@@ -199,40 +216,25 @@
}
void Log_data(){
- 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);
+ fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",roll,pitch,yaw,alt,input_roll,input_pitch,input_yaw,input_thr,velx,vely,velz);
}
void send_Blue(){
- 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 = ',';
+ if (send_ok == 1){
+ send_ok = 0;
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
trans_blue_data(roll,3,2);
- while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
trans_blue_data(pitch,3,2);
- while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->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';
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+ trans_blue_data(alt,2,2);
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+ trans_blue_data(velxyz,2,2);
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+ trans_blue_data(velz,2,2);
+ while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
}
}
@@ -244,80 +246,22 @@
int main(void)
{
AHRS.baud(9600);
- Blue_GPS.baud(9600);
- Blue_GPS.attach(&GPS_isr);
+ Blue.baud(9600);
+ Blue.attach(&Blue_isr);
AHRS.attach(&AHRS_isr);
- blue_trig.attach(&blue_trig_isr, 0.125);
- Micro_gf = gf_value; //Initial setting of servo
+ blue_trig.attach(&blue_trig_isr, 0.1);
+ neutral();
+ Log_file();
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(&alt, &del_alt);
- calb_alt();
- send_Blue();
- if (100<count) {stat=2;}
- break;
- 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);
- get_Baro(&alt, &del_alt);
- alt = alt - alt_zero;
- send_Blue();
- if (100.0<alt_GPS && del_alt>1){
- stat=3;
- Log_file();
- Micro_unf = unf_value;
- unf_value = 0.0; //checking
- 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(&alt, &del_alt);
+ get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
+ get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
+ get_Baro(&alt);
+ if (input_roll==0){input_roll = roll;}
+ if (input_pitch==0){input_pitch = pitch;}
+ if (input_yaw==0){input_yaw = yaw;}
+ ctrl_attitude();
Log_data();
- alt = alt - alt_zero;
send_Blue();
- 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(&alt, &del_alt);
- Log_data();
- alt = alt - alt_zero;
- send_Blue();
- 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(&alt, &del_alt);
- Log_data();
- alt = alt - alt_zero;
- send_Blue();
- end.read();
- if (end.read()>=10) {
- fclose(fp);
- stat=6;
- }
- break;
- case 6 : //Shut down
- break;
- }
+ if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){fclose(fp); break;}
}
}
\ No newline at end of file