CANSAT_AIRFUL / Mbed 2 deprecated mbed_paper_exponential

step3

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@1:cd11c1c592c7, 2016-07-19 (annotated)

Committer:
Soyoon
Date:
Tue Jul 19 14:30:51 2016 +0000
Revision:
1:cd11c1c592c7
Parent:
0:6ac6b2d2bf1a
Child:
2:9d0f979369cf
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soyoon 0:6ac6b2d2bf1a 1 #include "mbed.h"
Soyoon 0:6ac6b2d2bf1a 2 #include "Barometer.h"
Soyoon 0:6ac6b2d2bf1a 3 #include "Ultrasonic.h"
Soyoon 1:cd11c1c592c7 4 #include "LocalFileSystem.h"
Soyoon 0:6ac6b2d2bf1a 5 #include "math.h"
Soyoon 1:cd11c1c592c7 6
Soyoon 0:6ac6b2d2bf1a 7 Serial pc(USBTX, USBRX);
Soyoon 1:cd11c1c592c7 8 int data[14];
Soyoon 1:cd11c1c592c7 9 int stat=1;
Soyoon 0:6ac6b2d2bf1a 10
Soyoon 1:cd11c1c592c7 11 ////////////////////////////////////
Soyoon 1:cd11c1c592c7 12 // GPS 5V p27(RX) //
Soyoon 1:cd11c1c592c7 13 // Bluetooth 3.3V p28(TX) //
Soyoon 1:cd11c1c592c7 14 ////////////////////////////////////
Soyoon 1:cd11c1c592c7 15 Serial Blue_GPS(p28, p27);
Soyoon 1:cd11c1c592c7 16 char msg[150], ns, ew;
Soyoon 1:cd11c1c592c7 17 int i = 0, j=0, gps_ok=0, flag;
Soyoon 0:6ac6b2d2bf1a 18 volatile unsigned char GPS_buffer[2];
Soyoon 1:cd11c1c592c7 19 float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
Soyoon 0:6ac6b2d2bf1a 20
Soyoon 0:6ac6b2d2bf1a 21 void GPS_isr(){
Soyoon 0:6ac6b2d2bf1a 22 i++;
Soyoon 0:6ac6b2d2bf1a 23 GPS_buffer[1] = GPS_buffer[0];
Soyoon 1:cd11c1c592c7 24 GPS_buffer[0] = Blue_GPS.getc();
Soyoon 1:cd11c1c592c7 25 if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
Soyoon 0:6ac6b2d2bf1a 26 i=0;
Soyoon 1:cd11c1c592c7 27 if (flag == 1){flag = 0;gps_ok = 1;j=0;}
Soyoon 0:6ac6b2d2bf1a 28 }
Soyoon 0:6ac6b2d2bf1a 29 if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
Soyoon 0:6ac6b2d2bf1a 30 if (flag==1){msg[j]=GPS_buffer[0]; j++;}
Soyoon 0:6ac6b2d2bf1a 31 }
Soyoon 0:6ac6b2d2bf1a 32
Soyoon 0:6ac6b2d2bf1a 33 float trunc(float v) {
Soyoon 0:6ac6b2d2bf1a 34 if(v < 0.0) {
Soyoon 0:6ac6b2d2bf1a 35 v*= -1.0;
Soyoon 0:6ac6b2d2bf1a 36 v = floor(v);
Soyoon 0:6ac6b2d2bf1a 37 v*=-1.0;
Soyoon 1:cd11c1c592c7 38 }
Soyoon 1:cd11c1c592c7 39 else {v = floor(v);}
Soyoon 0:6ac6b2d2bf1a 40 return v;
Soyoon 0:6ac6b2d2bf1a 41 }
Soyoon 0:6ac6b2d2bf1a 42
Soyoon 1:cd11c1c592c7 43 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)
Soyoon 1:cd11c1c592c7 44 {
Soyoon 1:cd11c1c592c7 45 if (gps_ok == 1){
Soyoon 1:cd11c1c592c7 46 gps_ok = 0;
Soyoon 1:cd11c1c592c7 47 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);
Soyoon 1:cd11c1c592c7 48 if(*ns == 'S') {*latitude *= -1.0; }
Soyoon 1:cd11c1c592c7 49 if(*ew == 'W') {*longitude *= -1.0; }
Soyoon 1:cd11c1c592c7 50 float degrees = trunc(*latitude / 100.0f);
Soyoon 1:cd11c1c592c7 51 float minutes = (*latitude - (degrees * 100.0f));
Soyoon 1:cd11c1c592c7 52 *latitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 53 degrees = trunc(*longitude / 100.0f);
Soyoon 1:cd11c1c592c7 54 minutes = *longitude - (degrees * 100.0f);
Soyoon 1:cd11c1c592c7 55 *longitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 56 *Kor_time = *Kor_time + 90000;
Soyoon 1:cd11c1c592c7 57 }
Soyoon 1:cd11c1c592c7 58 }
Soyoon 1:cd11c1c592c7 59
Soyoon 1:cd11c1c592c7 60 ///////////////////////////////////////
Soyoon 1:cd11c1c592c7 61 // AHRS 5V p14(RX) //
Soyoon 1:cd11c1c592c7 62 ///////////////////////////////////////
Soyoon 1:cd11c1c592c7 63 Serial AHRS(p13, p14);
Soyoon 1:cd11c1c592c7 64 float roll,pitch,yaw,accx,accy,accz;
Soyoon 1:cd11c1c592c7 65 float t, alt;
Soyoon 1:cd11c1c592c7 66
Soyoon 1:cd11c1c592c7 67 void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
Soyoon 1:cd11c1c592c7 68 {
Soyoon 1:cd11c1c592c7 69 while(AHRS.getc() != '\n');
Soyoon 1:cd11c1c592c7 70 AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", roll, pitch, yaw, accx, accy, accz);
Soyoon 1:cd11c1c592c7 71 }
Soyoon 1:cd11c1c592c7 72
Soyoon 1:cd11c1c592c7 73 ////////////////////////////////////////////
Soyoon 1:cd11c1c592c7 74 // Barometer 3.3V p9(SDA) p10(SCL) //
Soyoon 1:cd11c1c592c7 75 ////////////////////////////////////////////
Soyoon 1:cd11c1c592c7 76 Barometer barometer(p9, p10);
Soyoon 1:cd11c1c592c7 77 float alt_sum=0.0f, alt_zero=0.0f, alt_avg=0.0f; // for zero-calibration
Soyoon 1:cd11c1c592c7 78 float alt_buffer[5];
Soyoon 1:cd11c1c592c7 79 int count = 0;
Soyoon 1:cd11c1c592c7 80
Soyoon 1:cd11c1c592c7 81 void get_Baro(float*t, float*alt)
Soyoon 1:cd11c1c592c7 82 {
Soyoon 1:cd11c1c592c7 83 barometer.update();
Soyoon 1:cd11c1c592c7 84 *t = barometer.get_temperature();
Soyoon 1:cd11c1c592c7 85 *alt = barometer.get_altitude_m();
Soyoon 1:cd11c1c592c7 86 }
Soyoon 1:cd11c1c592c7 87
Soyoon 1:cd11c1c592c7 88 void Baro_zero(){
Soyoon 1:cd11c1c592c7 89 if (count<=4){
Soyoon 1:cd11c1c592c7 90 alt_sum = alt_sum + alt;
Soyoon 1:cd11c1c592c7 91 count++;
Soyoon 1:cd11c1c592c7 92 }
Soyoon 1:cd11c1c592c7 93 else {
Soyoon 1:cd11c1c592c7 94 pc.printf("calibration\r\n");
Soyoon 1:cd11c1c592c7 95 alt_zero = alt_sum / (float)count ;
Soyoon 1:cd11c1c592c7 96 }
Soyoon 1:cd11c1c592c7 97 }
Soyoon 1:cd11c1c592c7 98
Soyoon 1:cd11c1c592c7 99 /*
Soyoon 1:cd11c1c592c7 100 void Baro_check(){
Soyoon 1:cd11c1c592c7 101 alt_buffer[0] = alt;
Soyoon 1:cd11c1c592c7 102 for(i=0;i<=3;i++) {alt_buffer[i+1] = alt_buffer[i]; } //Save in 5 buffers
Soyoon 1:cd11c1c592c7 103 del_alt = alt_min - alt_max;
Soyoon 1:cd11c1c592c7 104 }
Soyoon 1:cd11c1c592c7 105 */
Soyoon 1:cd11c1c592c7 106
Soyoon 1:cd11c1c592c7 107 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 108 // Servo 5V PWM //
Soyoon 1:cd11c1c592c7 109 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 110 PwmOut Linear(p21);
Soyoon 1:cd11c1c592c7 111 PwmOut Micro_gf(p22);
Soyoon 1:cd11c1c592c7 112 PwmOut Micro_unf(p23);
Soyoon 1:cd11c1c592c7 113
Soyoon 1:cd11c1c592c7 114 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 115 // Checksum //
Soyoon 1:cd11c1c592c7 116 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 117 long chksum;
Soyoon 1:cd11c1c592c7 118 unsigned int checksum (int data, short message_length)
Soyoon 1:cd11c1c592c7 119 {
Soyoon 1:cd11c1c592c7 120 unsigned int sum = 10;
Soyoon 1:cd11c1c592c7 121 /*for (i=0;i<=message_length; i++){
Soyoon 1:cd11c1c592c7 122 sum += data[i];
Soyoon 1:cd11c1c592c7 123 }
Soyoon 1:cd11c1c592c7 124 sum = (sum & 0xff);*/
Soyoon 1:cd11c1c592c7 125 return (unsigned int)(~sum);
Soyoon 1:cd11c1c592c7 126 }
Soyoon 1:cd11c1c592c7 127
Soyoon 1:cd11c1c592c7 128 void packet_data(){
Soyoon 1:cd11c1c592c7 129 data[0] = stat;
Soyoon 1:cd11c1c592c7 130 data[1] = Kor_time;
Soyoon 1:cd11c1c592c7 131 data[2] = (int)(alt*100);
Soyoon 1:cd11c1c592c7 132 data[3] = (int)(alt_GPS*100);
Soyoon 1:cd11c1c592c7 133 data[4] = (int)(roll*100);
Soyoon 1:cd11c1c592c7 134 data[5] = (int)(pitch*100);
Soyoon 1:cd11c1c592c7 135 data[6] = (int)(yaw*100);
Soyoon 1:cd11c1c592c7 136 data[7] = (int)(accz*100);
Soyoon 1:cd11c1c592c7 137 data[8] = (int)(latitude*100);
Soyoon 1:cd11c1c592c7 138 data[9] = (int)(longitude*100);
Soyoon 1:cd11c1c592c7 139 data[10] = Micro_unf*100;
Soyoon 1:cd11c1c592c7 140 data[11] = Micro_gf*100;
Soyoon 1:cd11c1c592c7 141 data[12] = Linear*100;
Soyoon 1:cd11c1c592c7 142 data[13] = chksum;
Soyoon 1:cd11c1c592c7 143 }
Soyoon 1:cd11c1c592c7 144
Soyoon 1:cd11c1c592c7 145 ///////////////////////////////
Soyoon 1:cd11c1c592c7 146 // Datalogger Mbed //
Soyoon 1:cd11c1c592c7 147 ///////////////////////////////
Soyoon 1:cd11c1c592c7 148 LocalFileSystem local("local");
Soyoon 1:cd11c1c592c7 149 int num=0;
Soyoon 1:cd11c1c592c7 150 FILE *fp;
Soyoon 1:cd11c1c592c7 151
Soyoon 1:cd11c1c592c7 152 void Log_data(){
Soyoon 1:cd11c1c592c7 153 FILE *fp = fopen("/local/data.txt", "w");
Soyoon 1:cd11c1c592c7 154 for (num=0;num<=13;num++) {fprintf(fp, "%i ",data[num]);}
Soyoon 1:cd11c1c592c7 155 num=0;
Soyoon 1:cd11c1c592c7 156 fprintf(fp, "\r\n");
Soyoon 1:cd11c1c592c7 157 }
Soyoon 1:cd11c1c592c7 158
Soyoon 0:6ac6b2d2bf1a 159 int main(void)
Soyoon 1:cd11c1c592c7 160 {
Soyoon 1:cd11c1c592c7 161 wait(10);
Soyoon 1:cd11c1c592c7 162 AHRS.baud(9600);
Soyoon 1:cd11c1c592c7 163 Blue_GPS.baud(9600);
Soyoon 1:cd11c1c592c7 164 Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
Soyoon 1:cd11c1c592c7 165 Linear.period(0.01); Micro_gf.period(0.01); Micro_unf.period(0.01);
Soyoon 1:cd11c1c592c7 166 Linear=0.19; Micro_unf=0.2; Micro_gf=0.2;//Initial setting of servo
Soyoon 1:cd11c1c592c7 167 //Linear=0.1;wait(1);Take back
Soyoon 1:cd11c1c592c7 168 pc.printf("Start\r\n");
Soyoon 1:cd11c1c592c7 169 while(1) {
Soyoon 1:cd11c1c592c7 170 switch(stat){
Soyoon 1:cd11c1c592c7 171 case 1 : //Bluetooth connection and calibration
Soyoon 1:cd11c1c592c7 172 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 173 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 174 get_Baro(&t, &alt);
Soyoon 1:cd11c1c592c7 175 Baro_zero();
Soyoon 1:cd11c1c592c7 176 packet_data();
Soyoon 1:cd11c1c592c7 177 //Log_data();
Soyoon 1:cd11c1c592c7 178 pc.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero);
Soyoon 1:cd11c1c592c7 179 //if (Kor_time!=0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
Soyoon 1:cd11c1c592c7 180 if (5<=count){Baro_zero(); pc.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero); stat=2;}
Soyoon 1:cd11c1c592c7 181 break;
Soyoon 1:cd11c1c592c7 182 case 2 : //Waiting launch
Soyoon 1:cd11c1c592c7 183 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 184 get_Baro(&t, &alt);
Soyoon 1:cd11c1c592c7 185 alt = alt - alt_zero;
Soyoon 1:cd11c1c592c7 186 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 187 packet_data();
Soyoon 1:cd11c1c592c7 188 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]);
Soyoon 1:cd11c1c592c7 189 if (100.0<alt_GPS && accz<0){stat=3;}
Soyoon 1:cd11c1c592c7 190 break;
Soyoon 1:cd11c1c592c7 191 case 3 : //Grid fin and landing leg spreading and control
Soyoon 1:cd11c1c592c7 192 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 193 get_Baro(&t, &alt);
Soyoon 1:cd11c1c592c7 194 alt = alt - alt_zero;
Soyoon 1:cd11c1c592c7 195 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 196 packet_data();
Soyoon 1:cd11c1c592c7 197 if (alt<3){stat=4;}
Soyoon 1:cd11c1c592c7 198 break;
Soyoon 1:cd11c1c592c7 199 case 4 : //Reverse thrust for soft landing
Soyoon 1:cd11c1c592c7 200 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 201 get_Baro(&t, &alt);
Soyoon 1:cd11c1c592c7 202 alt = alt - alt_zero;
Soyoon 1:cd11c1c592c7 203 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 204 packet_data();
Soyoon 1:cd11c1c592c7 205 if (accx<0.1 && accy<0.1 && accz<0.1) {stat=5;}
Soyoon 1:cd11c1c592c7 206 break;
Soyoon 1:cd11c1c592c7 207 case 5 : //Landing
Soyoon 1:cd11c1c592c7 208 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 209 get_Baro(&t, &alt);
Soyoon 1:cd11c1c592c7 210 alt = alt - alt_zero;
Soyoon 1:cd11c1c592c7 211 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 212 packet_data();
Soyoon 1:cd11c1c592c7 213 break;
Soyoon 0:6ac6b2d2bf1a 214 }
Soyoon 1:cd11c1c592c7 215 }
Soyoon 1:cd11c1c592c7 216 }