Kim Youngsik / Mbed 2 deprecated mbed_droptest2

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.

Dependencies:   Servo mbed

Fork of mbed_droptest by Soyoon Kim

main.cpp@2:391e8bf671ef, 2016-07-30 (annotated)

Committer:
Soyoon
Date:
Sat Jul 30 16:42:10 2016 +0000
Revision:
2:391e8bf671ef
Parent:
1:cd11c1c592c7
Child:
3:880d00d555c1
Airful;

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 1:cd11c1c592c7 3 #include "LocalFileSystem.h"
Soyoon 0:6ac6b2d2bf1a 4 #include "math.h"
Soyoon 2:391e8bf671ef 5 #include "Servo.h"
Soyoon 1:cd11c1c592c7 6
Soyoon 0:6ac6b2d2bf1a 7 Serial pc(USBTX, USBRX);
Soyoon 2:391e8bf671ef 8 Timer end;
Soyoon 1:cd11c1c592c7 9 int stat=1;
Soyoon 0:6ac6b2d2bf1a 10
Soyoon 1:cd11c1c592c7 11 ////////////////////////////////////
Soyoon 2:391e8bf671ef 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 2:391e8bf671ef 16
Soyoon 2:391e8bf671ef 17 char GPS_msg[150], ns, ew;
Soyoon 2:391e8bf671ef 18 int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
Soyoon 0:6ac6b2d2bf1a 19 volatile unsigned char GPS_buffer[2];
Soyoon 1:cd11c1c592c7 20 float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
Soyoon 0:6ac6b2d2bf1a 21
Soyoon 0:6ac6b2d2bf1a 22 void GPS_isr(){
Soyoon 0:6ac6b2d2bf1a 23 i++;
Soyoon 0:6ac6b2d2bf1a 24 GPS_buffer[1] = GPS_buffer[0];
Soyoon 1:cd11c1c592c7 25 GPS_buffer[0] = Blue_GPS.getc();
Soyoon 1:cd11c1c592c7 26 if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){
Soyoon 0:6ac6b2d2bf1a 27 i=0;
Soyoon 2:391e8bf671ef 28 if (GPS_flag == 1){GPS_flag = 0;gps_ok = 1;j=0;}
Soyoon 0:6ac6b2d2bf1a 29 }
Soyoon 2:391e8bf671ef 30 if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
Soyoon 2:391e8bf671ef 31 if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
Soyoon 0:6ac6b2d2bf1a 32 }
Soyoon 0:6ac6b2d2bf1a 33
Soyoon 0:6ac6b2d2bf1a 34 float trunc(float v) {
Soyoon 0:6ac6b2d2bf1a 35 if(v < 0.0) {
Soyoon 0:6ac6b2d2bf1a 36 v*= -1.0;
Soyoon 0:6ac6b2d2bf1a 37 v = floor(v);
Soyoon 0:6ac6b2d2bf1a 38 v*=-1.0;
Soyoon 1:cd11c1c592c7 39 }
Soyoon 1:cd11c1c592c7 40 else {v = floor(v);}
Soyoon 0:6ac6b2d2bf1a 41 return v;
Soyoon 0:6ac6b2d2bf1a 42 }
Soyoon 0:6ac6b2d2bf1a 43
Soyoon 1:cd11c1c592c7 44 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 45 {
Soyoon 1:cd11c1c592c7 46 if (gps_ok == 1){
Soyoon 1:cd11c1c592c7 47 gps_ok = 0;
Soyoon 2:391e8bf671ef 48 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);
Soyoon 1:cd11c1c592c7 49 if(*ns == 'S') {*latitude *= -1.0; }
Soyoon 1:cd11c1c592c7 50 if(*ew == 'W') {*longitude *= -1.0; }
Soyoon 1:cd11c1c592c7 51 float degrees = trunc(*latitude / 100.0f);
Soyoon 1:cd11c1c592c7 52 float minutes = (*latitude - (degrees * 100.0f));
Soyoon 1:cd11c1c592c7 53 *latitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 54 degrees = trunc(*longitude / 100.0f);
Soyoon 1:cd11c1c592c7 55 minutes = *longitude - (degrees * 100.0f);
Soyoon 1:cd11c1c592c7 56 *longitude = degrees + minutes / 60.0f;
Soyoon 1:cd11c1c592c7 57 *Kor_time = *Kor_time + 90000;
Soyoon 1:cd11c1c592c7 58 }
Soyoon 1:cd11c1c592c7 59 }
Soyoon 1:cd11c1c592c7 60
Soyoon 2:391e8bf671ef 61 //Bluetooth code is placed under the Log_data
Soyoon 2:391e8bf671ef 62
Soyoon 2:391e8bf671ef 63 ////////////////////////////////////////////
Soyoon 2:391e8bf671ef 64 // Barometer 3.3V p9(SDA) p10(SCL) //
Soyoon 2:391e8bf671ef 65 ////////////////////////////////////////////
Soyoon 2:391e8bf671ef 66 Barometer barometer(p9, p10);
Soyoon 2:391e8bf671ef 67 float t, alt, del_alt;
Soyoon 2:391e8bf671ef 68 float alt_sum=0.0f, alt_zero=0.0f;
Soyoon 2:391e8bf671ef 69 int count = 0, baro_ok = 0; // for zero-calibration
Soyoon 2:391e8bf671ef 70 float alt_buffer[4], w_alt=0.6; // weight for LPF
Soyoon 2:391e8bf671ef 71
Soyoon 2:391e8bf671ef 72 void get_Baro(float*t, float*alt)
Soyoon 2:391e8bf671ef 73 {
Soyoon 2:391e8bf671ef 74 if (baro_ok==1){
Soyoon 2:391e8bf671ef 75 barometer.update();
Soyoon 2:391e8bf671ef 76 *t = barometer.get_temperature();
Soyoon 2:391e8bf671ef 77 *alt = barometer.get_altitude_m();
Soyoon 2:391e8bf671ef 78 baro_ok = 0;
Soyoon 2:391e8bf671ef 79 }
Soyoon 2:391e8bf671ef 80 }
Soyoon 2:391e8bf671ef 81
Soyoon 2:391e8bf671ef 82 void calb_alt(){
Soyoon 2:391e8bf671ef 83 if (alt==0){}
Soyoon 2:391e8bf671ef 84 else {
Soyoon 2:391e8bf671ef 85 if (count<=19){alt_sum = alt_sum + alt; count++;}
Soyoon 2:391e8bf671ef 86 else {
Soyoon 2:391e8bf671ef 87 Blue_GPS.printf("calibration\r\n");
Soyoon 2:391e8bf671ef 88 alt_zero = alt_sum / (float)count;
Soyoon 2:391e8bf671ef 89 count++;
Soyoon 2:391e8bf671ef 90 }
Soyoon 2:391e8bf671ef 91 }
Soyoon 2:391e8bf671ef 92 }
Soyoon 2:391e8bf671ef 93
Soyoon 2:391e8bf671ef 94 void lpf_alt(){
Soyoon 2:391e8bf671ef 95 alt_buffer[1] = alt_buffer[0];
Soyoon 2:391e8bf671ef 96 alt_buffer[0] = alt;
Soyoon 2:391e8bf671ef 97 alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 98 alt_buffer[3] = alt_buffer[2];
Soyoon 2:391e8bf671ef 99 alt_buffer[2] = alt;
Soyoon 2:391e8bf671ef 100 del_alt = alt_buffer[3]-alt_buffer[2]; // for calculation of drop speed
Soyoon 2:391e8bf671ef 101 }
Soyoon 2:391e8bf671ef 102
Soyoon 1:cd11c1c592c7 103 ///////////////////////////////////////
Soyoon 2:391e8bf671ef 104 // AHRS 5V p14(RX) // 20Hz
Soyoon 1:cd11c1c592c7 105 ///////////////////////////////////////
Soyoon 1:cd11c1c592c7 106 Serial AHRS(p13, p14);
Soyoon 1:cd11c1c592c7 107 float roll,pitch,yaw,accx,accy,accz;
Soyoon 2:391e8bf671ef 108 char AHRS_msg[150];
Soyoon 2:391e8bf671ef 109 int m=0, ahrs_ok=0, AHRS_flag;
Soyoon 2:391e8bf671ef 110 volatile unsigned char AHRS_buffer[2];
Soyoon 2:391e8bf671ef 111 float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
Soyoon 2:391e8bf671ef 112
Soyoon 2:391e8bf671ef 113 void AHRS_isr(){ //inturupt
Soyoon 2:391e8bf671ef 114 while(AHRS.readable()){
Soyoon 2:391e8bf671ef 115 AHRS_buffer[1] = AHRS_buffer[0];
Soyoon 2:391e8bf671ef 116 AHRS_buffer[0] = AHRS.getc();
Soyoon 2:391e8bf671ef 117 if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
Soyoon 2:391e8bf671ef 118 if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
Soyoon 2:391e8bf671ef 119 if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
Soyoon 2:391e8bf671ef 120 }
Soyoon 2:391e8bf671ef 121 }
Soyoon 1:cd11c1c592c7 122
Soyoon 1:cd11c1c592c7 123 void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
Soyoon 1:cd11c1c592c7 124 {
Soyoon 2:391e8bf671ef 125 if (ahrs_ok == 1){
Soyoon 2:391e8bf671ef 126 ahrs_ok = 0;
Soyoon 2:391e8bf671ef 127 sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
Soyoon 2:391e8bf671ef 128 baro_ok = 1;
Soyoon 2:391e8bf671ef 129 }
Soyoon 2:391e8bf671ef 130 }
Soyoon 1:cd11c1c592c7 131
Soyoon 2:391e8bf671ef 132 void lpf_attitude(){
Soyoon 2:391e8bf671ef 133 roll_buffer[1] = roll_buffer[0];
Soyoon 2:391e8bf671ef 134 roll_buffer[0] = roll;
Soyoon 2:391e8bf671ef 135 roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 136 pitch_buffer[1] = pitch_buffer[0];
Soyoon 2:391e8bf671ef 137 pitch_buffer[0] = pitch;
Soyoon 2:391e8bf671ef 138 pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0]; // Low Pass Filter
Soyoon 2:391e8bf671ef 139 yaw_buffer[1] = yaw_buffer[0];
Soyoon 2:391e8bf671ef 140 yaw_buffer[0] = yaw;
Soyoon 2:391e8bf671ef 141 yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0]; // Low Pass Filter
Soyoon 1:cd11c1c592c7 142 }
Soyoon 1:cd11c1c592c7 143
Soyoon 1:cd11c1c592c7 144
Soyoon 1:cd11c1c592c7 145 ///////////////////////////////////
Soyoon 1:cd11c1c592c7 146 // Servo 5V PWM //
Soyoon 1:cd11c1c592c7 147 ///////////////////////////////////
Soyoon 2:391e8bf671ef 148 Servo Micro_gf(p22);
Soyoon 1:cd11c1c592c7 149
Soyoon 2:391e8bf671ef 150 float unf_value=1.0, gf_value = 0.35,linear_value = 1.0;
Soyoon 2:391e8bf671ef 151 float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
Soyoon 2:391e8bf671ef 152 void ctl_gf(){
Soyoon 2:391e8bf671ef 153 err_yaw = yaw - tg_yaw;
Soyoon 2:391e8bf671ef 154 gf_value = 0.35*exp(0.0039*p*err_yaw);
Soyoon 2:391e8bf671ef 155 if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
Soyoon 2:391e8bf671ef 156 if (gf_value<=0.0){gf_value=0.0;}
Soyoon 2:391e8bf671ef 157 else if (gf_value>=0.7) {gf_value=0.7;}
Soyoon 2:391e8bf671ef 158 Micro_gf = gf_value;
Soyoon 1:cd11c1c592c7 159 }
Soyoon 1:cd11c1c592c7 160
Soyoon 1:cd11c1c592c7 161 ///////////////////////////////
Soyoon 1:cd11c1c592c7 162 // Datalogger Mbed //
Soyoon 1:cd11c1c592c7 163 ///////////////////////////////
Soyoon 1:cd11c1c592c7 164 LocalFileSystem local("local");
Soyoon 2:391e8bf671ef 165 int file_no=1;
Soyoon 2:391e8bf671ef 166 char filename[256];
Soyoon 1:cd11c1c592c7 167 FILE *fp;
Soyoon 1:cd11c1c592c7 168
Soyoon 2:391e8bf671ef 169 void Log_file(){
Soyoon 2:391e8bf671ef 170 sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
Soyoon 2:391e8bf671ef 171 fp = fopen(filename, "r"); // if file can be loaded, return is 1
Soyoon 2:391e8bf671ef 172 while(fp){
Soyoon 2:391e8bf671ef 173 fclose(fp);
Soyoon 2:391e8bf671ef 174 file_no ++;
Soyoon 2:391e8bf671ef 175 sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing
Soyoon 2:391e8bf671ef 176 fp = fopen(filename, "r");
Soyoon 2:391e8bf671ef 177 }
Soyoon 2:391e8bf671ef 178 fp = fopen(filename, "w");
Soyoon 2:391e8bf671ef 179 }
Soyoon 2:391e8bf671ef 180
Soyoon 1:cd11c1c592c7 181 void Log_data(){
Soyoon 2:391e8bf671ef 182 fprintf(fp, "%i,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
Soyoon 1:cd11c1c592c7 183 }
Soyoon 1:cd11c1c592c7 184
Soyoon 2:391e8bf671ef 185 void send_Blue(){
Soyoon 2:391e8bf671ef 186 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);
Soyoon 2:391e8bf671ef 187 }
Soyoon 2:391e8bf671ef 188
Soyoon 2:391e8bf671ef 189 /////////////////////////////////
Soyoon 2:391e8bf671ef 190 // Main loop //
Soyoon 2:391e8bf671ef 191 /////////////////////////////////
Soyoon 2:391e8bf671ef 192
Soyoon 0:6ac6b2d2bf1a 193 int main(void)
Soyoon 1:cd11c1c592c7 194 {
Soyoon 1:cd11c1c592c7 195 AHRS.baud(9600);
Soyoon 1:cd11c1c592c7 196 Blue_GPS.baud(9600);
Soyoon 1:cd11c1c592c7 197 Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
Soyoon 2:391e8bf671ef 198 AHRS.attach(&AHRS_isr, Serial::RxIrq);
Soyoon 2:391e8bf671ef 199 Blue_GPS.printf("Start\r\n");
Soyoon 2:391e8bf671ef 200 //Log_file();
Soyoon 2:391e8bf671ef 201 gf_value = 0.7;
Soyoon 2:391e8bf671ef 202 Micro_gf = gf_value;
Soyoon 1:cd11c1c592c7 203 while(1) {
Soyoon 1:cd11c1c592c7 204 switch(stat){
Soyoon 2:391e8bf671ef 205 case 1 : //Calibration
Soyoon 1:cd11c1c592c7 206 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 207 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 208 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 209 calb_alt();
Soyoon 2:391e8bf671ef 210 lpf_attitude();
Soyoon 2:391e8bf671ef 211 Blue_GPS.printf("%i %f %f %f\r\n",count, alt, alt_sum, alt_zero);
Soyoon 2:391e8bf671ef 212 if (20<count) {
Soyoon 2:391e8bf671ef 213 stat=2;
Soyoon 2:391e8bf671ef 214 }
Soyoon 1:cd11c1c592c7 215 break;
Soyoon 2:391e8bf671ef 216 case 2 : //Wait
Soyoon 1:cd11c1c592c7 217 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 218 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 219 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 220 //Log_data();
Soyoon 1:cd11c1c592c7 221 alt = alt - alt_zero;
Soyoon 2:391e8bf671ef 222 lpf_alt();
Soyoon 2:391e8bf671ef 223 lpf_attitude();
Soyoon 2:391e8bf671ef 224 send_Blue();
Soyoon 2:391e8bf671ef 225 if (alt<=-10 && abs(accx)<0.1 && abs(accy)<0.1){
Soyoon 2:391e8bf671ef 226 //fclose(fp);
Soyoon 2:391e8bf671ef 227 stat=3; send_Blue();
Soyoon 2:391e8bf671ef 228 }
Soyoon 2:391e8bf671ef 229 break;
Soyoon 2:391e8bf671ef 230 /*case 3 : //Drop
Soyoon 1:cd11c1c592c7 231 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 1:cd11c1c592c7 232 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 233 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 234 //Log_data();
Soyoon 2:391e8bf671ef 235 lpf_alt();
Soyoon 1:cd11c1c592c7 236 alt = alt - alt_zero;
Soyoon 2:391e8bf671ef 237 lpf_attitude();
Soyoon 2:391e8bf671ef 238 send_Blue();
Soyoon 2:391e8bf671ef 239 pc.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);
Soyoon 2:391e8bf671ef 240 if (alt<=5 && abs(accx)<0.1 && abs(accy)<0.1) {end.start(); stat=4;}
Soyoon 1:cd11c1c592c7 241 break;
Soyoon 2:391e8bf671ef 242 case 4 : //Landing
Soyoon 1:cd11c1c592c7 243 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
Soyoon 1:cd11c1c592c7 244 get_Baro(&t, &alt);
Soyoon 2:391e8bf671ef 245 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
Soyoon 2:391e8bf671ef 246 //Log_data();
Soyoon 2:391e8bf671ef 247 lpf_alt();
Soyoon 1:cd11c1c592c7 248 alt = alt - alt_zero;
Soyoon 2:391e8bf671ef 249 lpf_attitude();
Soyoon 2:391e8bf671ef 250 send_Blue();
Soyoon 2:391e8bf671ef 251 end.read();
Soyoon 2:391e8bf671ef 252 pc.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);
Soyoon 2:391e8bf671ef 253 if (end.read()>=10) {fclose(fp);}
Soyoon 2:391e8bf671ef 254 break;*/
Soyoon 2:391e8bf671ef 255 case 3 : //Shut down
Soyoon 1:cd11c1c592c7 256 break;
Soyoon 0:6ac6b2d2bf1a 257 }
Soyoon 1:cd11c1c592c7 258 }
Soyoon 1:cd11c1c592c7 259 }