2017.11伊豆大島共同打ち上げ実験のデータ取得&保存&無線送信用プログラム
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of 2017_11_1768 by
main.cpp@8:8b7e9aa3a2fc, 2017-11-10 (annotated)
- Committer:
- oichan
- Date:
- Fri Nov 10 13:14:11 2017 +0000
- Revision:
- 8:8b7e9aa3a2fc
- Parent:
- 7:5f8080485160
- Child:
- 9:97fd5ab1f7c3
flight model
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbed_official | 0:bdbd3d6fc5d5 | 1 | #include "mbed.h" |
oichan | 2:0cf17bba70ec | 2 | #include "math.h" |
oichan | 2:0cf17bba70ec | 3 | #include "MPU6050.h" |
oichan | 2:0cf17bba70ec | 4 | #include "BMP180.h" |
mbed_official | 0:bdbd3d6fc5d5 | 5 | #include "SDFileSystem.h" |
oichan | 2:0cf17bba70ec | 6 | |
oichan | 2:0cf17bba70ec | 7 | #define RATE 20 |
oichan | 2:0cf17bba70ec | 8 | #define MAX_JUDGE_TIME_s 3.0 |
oichan | 2:0cf17bba70ec | 9 | #define p0 1013.25f |
oichan | 2:0cf17bba70ec | 10 | |
oichan | 2:0cf17bba70ec | 11 | BMP180 bmp(p28,p27); |
oichan | 2:0cf17bba70ec | 12 | MPU6050 mpu(p28,p27); |
oichan | 2:0cf17bba70ec | 13 | Timer timer; |
oichan | 2:0cf17bba70ec | 14 | Timer alt_timer; |
oichan | 2:0cf17bba70ec | 15 | Ticker logtimer; |
oichan | 3:71a45bae8a37 | 16 | DigitalOut oshirase1(p19); |
oichan | 3:71a45bae8a37 | 17 | DigitalInOut oshirase2(p20); |
oichan | 6:aae92602ef84 | 18 | //Serial pc(USBTX,USBRX); |
oichan | 2:0cf17bba70ec | 19 | Serial gps(p13,p14); |
oichan | 2:0cf17bba70ec | 20 | Serial twe(p9,p10); |
oichan | 2:0cf17bba70ec | 21 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
oichan | 2:0cf17bba70ec | 22 | LocalFileSystem local("local"); |
oichan | 2:0cf17bba70ec | 23 | FILE *fp; |
oichan | 2:0cf17bba70ec | 24 | |
oichan | 2:0cf17bba70ec | 25 | /* 自作関数 */ |
oichan | 2:0cf17bba70ec | 26 | void _flight(); |
oichan | 2:0cf17bba70ec | 27 | void _log(); |
oichan | 2:0cf17bba70ec | 28 | float _getAlt(float press, float temp); |
oichan | 2:0cf17bba70ec | 29 | float _DMS2DEG(float raw_data); |
oichan | 2:0cf17bba70ec | 30 | float _median(float data[], int num); |
oichan | 6:aae92602ef84 | 31 | int _input(char moji); |
oichan | 2:0cf17bba70ec | 32 | |
oichan | 2:0cf17bba70ec | 33 | /* グローバル変数 */ |
oichan | 2:0cf17bba70ec | 34 | float a[3]; |
oichan | 2:0cf17bba70ec | 35 | float Land_Alt; |
oichan | 4:4920f8106896 | 36 | float Max_Alt = 0; |
oichan | 2:0cf17bba70ec | 37 | float Alt_buff[10]; |
oichan | 2:0cf17bba70ec | 38 | float Pressure,Temperature,Altitude; |
oichan | 2:0cf17bba70ec | 39 | float Time; |
oichan | 2:0cf17bba70ec | 40 | char gps_data[256]; |
oichan | 3:71a45bae8a37 | 41 | char c; |
oichan | 6:aae92602ef84 | 42 | char command; |
oichan | 4:4920f8106896 | 43 | int cnt_gps = 0; |
oichan | 4:4920f8106896 | 44 | int Log_cnt = 0; |
oichan | 4:4920f8106896 | 45 | int Phase = 0; |
oichan | 2:0cf17bba70ec | 46 | |
oichan | 2:0cf17bba70ec | 47 | |
mbed_official | 0:bdbd3d6fc5d5 | 48 | int main() { |
oichan | 8:8b7e9aa3a2fc | 49 | // pc.printf("Hello, Happy World!\r\n"); |
oichan | 6:aae92602ef84 | 50 | oshirase2.output(); |
oichan | 6:aae92602ef84 | 51 | oshirase1 = 0; |
oichan | 6:aae92602ef84 | 52 | oshirase2 = 0; |
oichan | 2:0cf17bba70ec | 53 | twe.baud(115200); |
oichan | 8:8b7e9aa3a2fc | 54 | twe.printf("Hello World!\r\n"); |
oichan | 2:0cf17bba70ec | 55 | mpu.setAcceleroRange(0); |
oichan | 2:0cf17bba70ec | 56 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
mbed_official | 0:bdbd3d6fc5d5 | 57 | mkdir("/sd/mydir", 0777); |
oichan | 4:4920f8106896 | 58 | while(1){ |
oichan | 6:aae92602ef84 | 59 | command = twe.getc(); |
oichan | 6:aae92602ef84 | 60 | if(_input(command)==-1){ //フライトモードへ移行 |
oichan | 3:71a45bae8a37 | 61 | oshirase1 = 0; |
oichan | 3:71a45bae8a37 | 62 | oshirase2 = 1; |
oichan | 4:4920f8106896 | 63 | _flight(); |
oichan | 4:4920f8106896 | 64 | break; |
oichan | 6:aae92602ef84 | 65 | }else if(_input(command)==1){ //サーボ動かすだけモードへ |
oichan | 3:71a45bae8a37 | 66 | oshirase1 = 1; |
oichan | 3:71a45bae8a37 | 67 | oshirase2 = 0; |
oichan | 3:71a45bae8a37 | 68 | while(1){ |
oichan | 6:aae92602ef84 | 69 | command = twe.getc(); |
oichan | 6:aae92602ef84 | 70 | if(_input(command)==3){ //開放用サーボ(MG996)を動かす |
oichan | 3:71a45bae8a37 | 71 | oshirase1 = 1; |
oichan | 3:71a45bae8a37 | 72 | oshirase2 = 1; |
oichan | 3:71a45bae8a37 | 73 | break; |
oichan | 6:aae92602ef84 | 74 | }else if(_input(command)==7){ //リーフィングのサーボ(SG92)を動かす |
oichan | 3:71a45bae8a37 | 75 | oshirase1 = 0; |
oichan | 3:71a45bae8a37 | 76 | oshirase2 = 0; |
oichan | 3:71a45bae8a37 | 77 | break; |
oichan | 3:71a45bae8a37 | 78 | } |
oichan | 3:71a45bae8a37 | 79 | } |
oichan | 3:71a45bae8a37 | 80 | } |
oichan | 3:71a45bae8a37 | 81 | } |
oichan | 4:4920f8106896 | 82 | while(1); |
oichan | 2:0cf17bba70ec | 83 | } |
oichan | 2:0cf17bba70ec | 84 | |
oichan | 2:0cf17bba70ec | 85 | |
oichan | 2:0cf17bba70ec | 86 | /* フライトモード用関数 */ |
oichan | 2:0cf17bba70ec | 87 | void _flight(){ |
oichan | 4:4920f8106896 | 88 | oshirase2.input(); |
oichan | 6:aae92602ef84 | 89 | twe.printf("flight mode on\r\n"); |
oichan | 6:aae92602ef84 | 90 | // pc.printf("flight mode on\r\n"); |
oichan | 2:0cf17bba70ec | 91 | |
oichan | 2:0cf17bba70ec | 92 | /* 地上高度取得 */ |
oichan | 2:0cf17bba70ec | 93 | for(int i=0;i<10;i++){ |
oichan | 2:0cf17bba70ec | 94 | bmp.ReadData(&Temperature,&Pressure); |
oichan | 2:0cf17bba70ec | 95 | Alt_buff[i] = _getAlt(Pressure,Temperature); |
oichan | 2:0cf17bba70ec | 96 | } |
oichan | 2:0cf17bba70ec | 97 | Land_Alt = _median(Alt_buff,10); |
oichan | 3:71a45bae8a37 | 98 | Max_Alt = Land_Alt; |
oichan | 2:0cf17bba70ec | 99 | FILE *lfp = fopen("/local/alt.txt","a"); |
oichan | 2:0cf17bba70ec | 100 | fprintf(lfp,"地上高度:%f\r\n",Land_Alt); |
oichan | 2:0cf17bba70ec | 101 | fclose(lfp); |
oichan | 6:aae92602ef84 | 102 | twe.printf("地上高度:%f\r\n",Land_Alt); |
oichan | 6:aae92602ef84 | 103 | // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
mbed_official | 0:bdbd3d6fc5d5 | 104 | |
oichan | 2:0cf17bba70ec | 105 | /* データ取得開始 */ |
oichan | 2:0cf17bba70ec | 106 | timer.start(); |
oichan | 2:0cf17bba70ec | 107 | logtimer.attach(_log,1.0/RATE); |
oichan | 2:0cf17bba70ec | 108 | |
oichan | 2:0cf17bba70ec | 109 | while(1){ |
oichan | 6:aae92602ef84 | 110 | |
oichan | 4:4920f8106896 | 111 | /* フェーズ管理 */ |
oichan | 4:4920f8106896 | 112 | if(Phase==0){ |
oichan | 4:4920f8106896 | 113 | if(oshirase2==1){ |
oichan | 5:1e66d892109b | 114 | Phase = 1; //上昇フェーズへ移行 |
oichan | 4:4920f8106896 | 115 | alt_timer.start(); |
oichan | 4:4920f8106896 | 116 | } |
oichan | 4:4920f8106896 | 117 | }else if(Phase==1){ |
oichan | 4:4920f8106896 | 118 | if(oshirase2==0){ |
oichan | 5:1e66d892109b | 119 | Phase = 2; //降下1フェーズへ移行 |
oichan | 4:4920f8106896 | 120 | } |
oichan | 4:4920f8106896 | 121 | }else if(Phase==2){ |
oichan | 4:4920f8106896 | 122 | if(oshirase2==1){ |
oichan | 5:1e66d892109b | 123 | Phase = 3; //降下2フェーズへ移行 |
oichan | 4:4920f8106896 | 124 | } |
oichan | 4:4920f8106896 | 125 | } |
oichan | 6:aae92602ef84 | 126 | |
oichan | 6:aae92602ef84 | 127 | /* 30分ごとにタイマーをリセット(オーバーフロー対策) */ |
oichan | 6:aae92602ef84 | 128 | if(Time > 60*30){ |
oichan | 6:aae92602ef84 | 129 | timer.reset(); |
oichan | 6:aae92602ef84 | 130 | } |
oichan | 8:8b7e9aa3a2fc | 131 | |
oichan | 8:8b7e9aa3a2fc | 132 | // twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt); |
oichan | 4:4920f8106896 | 133 | |
oichan | 4:4920f8106896 | 134 | /* GPS取得&送信 */ |
oichan | 8:8b7e9aa3a2fc | 135 | // twe.printf("0"); |
oichan | 2:0cf17bba70ec | 136 | gps_data[cnt_gps] = gps.getc(); |
oichan | 8:8b7e9aa3a2fc | 137 | // twe.printf("1"); |
oichan | 2:0cf17bba70ec | 138 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
oichan | 8:8b7e9aa3a2fc | 139 | // twe.printf("2"); |
oichan | 2:0cf17bba70ec | 140 | cnt_gps = 0; |
oichan | 2:0cf17bba70ec | 141 | memset(gps_data,'\0',256); |
oichan | 2:0cf17bba70ec | 142 | }else if(gps_data[cnt_gps] == '\r'){ |
oichan | 2:0cf17bba70ec | 143 | float world_time, lon_east, lat_north; |
oichan | 2:0cf17bba70ec | 144 | int rlock, sat_num; |
oichan | 2:0cf17bba70ec | 145 | char lat,lon; |
oichan | 8:8b7e9aa3a2fc | 146 | // twe.printf("3"); |
oichan | 2:0cf17bba70ec | 147 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
oichan | 2:0cf17bba70ec | 148 | if(rlock==1){ |
oichan | 8:8b7e9aa3a2fc | 149 | // twe.printf("4"); |
oichan | 2:0cf17bba70ec | 150 | lat_north = _DMS2DEG(lat_north); |
oichan | 2:0cf17bba70ec | 151 | lon_east = _DMS2DEG(lon_east); |
oichan | 2:0cf17bba70ec | 152 | twe.printf("%s\r\n",gps_data); |
oichan | 2:0cf17bba70ec | 153 | twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
oichan | 6:aae92602ef84 | 154 | twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt); |
oichan | 8:8b7e9aa3a2fc | 155 | }else{ |
oichan | 8:8b7e9aa3a2fc | 156 | // twe.printf("5"); |
oichan | 2:0cf17bba70ec | 157 | twe.printf("%s\r\n",gps_data); |
oichan | 8:8b7e9aa3a2fc | 158 | twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt); |
oichan | 2:0cf17bba70ec | 159 | } |
oichan | 2:0cf17bba70ec | 160 | } |
oichan | 2:0cf17bba70ec | 161 | }else{ |
oichan | 2:0cf17bba70ec | 162 | cnt_gps++; |
oichan | 8:8b7e9aa3a2fc | 163 | // twe.printf("6"); |
oichan | 2:0cf17bba70ec | 164 | } |
oichan | 8:8b7e9aa3a2fc | 165 | // twe.printf("Hi!"); |
oichan | 2:0cf17bba70ec | 166 | } |
oichan | 2:0cf17bba70ec | 167 | } |
oichan | 2:0cf17bba70ec | 168 | |
oichan | 2:0cf17bba70ec | 169 | |
oichan | 2:0cf17bba70ec | 170 | /* データ取得&保存関数 */ |
oichan | 2:0cf17bba70ec | 171 | void _log(){ |
oichan | 2:0cf17bba70ec | 172 | Time = timer.read(); |
oichan | 2:0cf17bba70ec | 173 | mpu.getAccelero(a); |
oichan | 2:0cf17bba70ec | 174 | bmp.ReadData(&Temperature,&Pressure); |
oichan | 2:0cf17bba70ec | 175 | Altitude = _getAlt(Pressure,Temperature); |
oichan | 4:4920f8106896 | 176 | |
oichan | 4:4920f8106896 | 177 | /* 最高高度取得&保存 */ |
oichan | 2:0cf17bba70ec | 178 | if(Altitude > Max_Alt){ |
oichan | 4:4920f8106896 | 179 | Max_Alt = Altitude; |
oichan | 4:4920f8106896 | 180 | alt_timer.reset(); |
oichan | 6:aae92602ef84 | 181 | }else if(alt_timer.read() > MAX_JUDGE_TIME_s){ |
oichan | 4:4920f8106896 | 182 | alt_timer.stop(); |
oichan | 2:0cf17bba70ec | 183 | alt_timer.reset(); |
oichan | 2:0cf17bba70ec | 184 | FILE *lfp = fopen("/local/alt.txt","a"); |
oichan | 2:0cf17bba70ec | 185 | fprintf(lfp,"最高高度:%f\r\n",Max_Alt); |
oichan | 2:0cf17bba70ec | 186 | fclose(lfp); |
oichan | 6:aae92602ef84 | 187 | // pc.printf("最高高度:%f\r\n",Max_Alt); |
oichan | 2:0cf17bba70ec | 188 | } |
oichan | 4:4920f8106896 | 189 | |
oichan | 4:4920f8106896 | 190 | /* データ保存 */ |
oichan | 2:0cf17bba70ec | 191 | if(Log_cnt==0) { |
oichan | 7:5f8080485160 | 192 | fp = fopen("/sd/mydir/log.txt", "a"); |
oichan | 2:0cf17bba70ec | 193 | } |
oichan | 4:4920f8106896 | 194 | fprintf(fp, "%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]); |
oichan | 2:0cf17bba70ec | 195 | Log_cnt++; |
oichan | 2:0cf17bba70ec | 196 | if(Log_cnt==RATE){ |
oichan | 2:0cf17bba70ec | 197 | fclose(fp); |
oichan | 2:0cf17bba70ec | 198 | Log_cnt = 0; |
oichan | 2:0cf17bba70ec | 199 | } |
oichan | 6:aae92602ef84 | 200 | // pc.printf("%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]); |
oichan | 2:0cf17bba70ec | 201 | } |
oichan | 2:0cf17bba70ec | 202 | |
oichan | 2:0cf17bba70ec | 203 | |
oichan | 2:0cf17bba70ec | 204 | /* 高度計算関数 */ |
oichan | 2:0cf17bba70ec | 205 | float _getAlt(float press, float temp){ |
oichan | 2:0cf17bba70ec | 206 | return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
mbed_official | 0:bdbd3d6fc5d5 | 207 | } |
oichan | 2:0cf17bba70ec | 208 | |
oichan | 2:0cf17bba70ec | 209 | |
oichan | 3:71a45bae8a37 | 210 | /* 緯度経度計算関数 */ |
oichan | 2:0cf17bba70ec | 211 | float _DMS2DEG(float raw_data){ |
oichan | 2:0cf17bba70ec | 212 | int d = (int)(raw_data/100); |
oichan | 2:0cf17bba70ec | 213 | float m = (raw_data - (float)d*100); |
oichan | 2:0cf17bba70ec | 214 | return (float)d + m/60; |
oichan | 2:0cf17bba70ec | 215 | } |
oichan | 2:0cf17bba70ec | 216 | |
oichan | 2:0cf17bba70ec | 217 | |
oichan | 2:0cf17bba70ec | 218 | /* 中央値計算関数 */ |
oichan | 2:0cf17bba70ec | 219 | float _median(float data[], int num){//todo:処理時間計測 |
oichan | 2:0cf17bba70ec | 220 | float *data_cpy, ans; |
oichan | 2:0cf17bba70ec | 221 | data_cpy = new float[num]; |
oichan | 2:0cf17bba70ec | 222 | memcpy(data_cpy,data,sizeof(float)*num); |
oichan | 2:0cf17bba70ec | 223 | |
oichan | 2:0cf17bba70ec | 224 | for(int i=0; i<num; i++){ |
oichan | 2:0cf17bba70ec | 225 | for(int j=0; j<num-i-1; j++){ |
oichan | 2:0cf17bba70ec | 226 | if(data_cpy[j]>data_cpy[j+1]){ |
oichan | 2:0cf17bba70ec | 227 | float buff = data_cpy[j+1]; |
oichan | 2:0cf17bba70ec | 228 | data_cpy[j+1] = data_cpy[j]; |
oichan | 2:0cf17bba70ec | 229 | data_cpy[j] = buff; |
oichan | 2:0cf17bba70ec | 230 | } |
oichan | 2:0cf17bba70ec | 231 | } |
oichan | 2:0cf17bba70ec | 232 | } |
oichan | 2:0cf17bba70ec | 233 | |
oichan | 2:0cf17bba70ec | 234 | if(num%2!=0) ans = data_cpy[num/2]; |
oichan | 2:0cf17bba70ec | 235 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
oichan | 2:0cf17bba70ec | 236 | delete[] data_cpy; |
oichan | 2:0cf17bba70ec | 237 | return ans; |
oichan | 2:0cf17bba70ec | 238 | } |
oichan | 3:71a45bae8a37 | 239 | |
oichan | 6:aae92602ef84 | 240 | /* コマンド判別用関数 */ |
oichan | 6:aae92602ef84 | 241 | int _input(char moji){ |
oichan | 6:aae92602ef84 | 242 | if(moji!='\0'){ |
oichan | 6:aae92602ef84 | 243 | twe.printf("%c\r\n",c); |
oichan | 8:8b7e9aa3a2fc | 244 | // pc.printf("%c\r\n"); |
oichan | 3:71a45bae8a37 | 245 | } |
oichan | 6:aae92602ef84 | 246 | if(moji=='f'){ |
oichan | 3:71a45bae8a37 | 247 | return -1; |
oichan | 6:aae92602ef84 | 248 | }else if(moji=='s'){ |
oichan | 4:4920f8106896 | 249 | return 1; |
oichan | 6:aae92602ef84 | 250 | }else if(moji=='p'){ |
oichan | 4:4920f8106896 | 251 | return 3; |
oichan | 6:aae92602ef84 | 252 | }else if(moji=='l'){ |
oichan | 4:4920f8106896 | 253 | return 7; |
oichan | 3:71a45bae8a37 | 254 | } |
oichan | 3:71a45bae8a37 | 255 | return 0; |
oichan | 5:1e66d892109b | 256 | } |