kohei oikawa / Mbed 2 deprecated 2017_11_1768

2017.11伊豆大島共同打ち上げ実験のデータ取得＆保存用プログラム

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

main.cpp@7:5f8080485160, 2017-11-05 (annotated)

Committer:
oichan
Date:
Sun Nov 05 09:41:40 2017 +0000
Revision:
7:5f8080485160
Parent:
6:aae92602ef84
Child:
8:8b7e9aa3a2fc
produced by oikawa

Who changed what in which revision?

UserRevisionLine numberNew 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 6:aae92602ef84 49 oshirase2.output();
oichan 6:aae92602ef84 50 oshirase1 = 0;
oichan 6:aae92602ef84 51 oshirase2 = 0;
oichan 2:0cf17bba70ec 52 twe.baud(115200);
oichan 7:5f8080485160 53 twe.printf("Hello World!\n");
oichan 2:0cf17bba70ec 54 mpu.setAcceleroRange(0);
oichan 2:0cf17bba70ec 55 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
mbed_official 0:bdbd3d6fc5d5 56 mkdir("/sd/mydir", 0777);
oichan 4:4920f8106896 57 while(1){
oichan 6:aae92602ef84 58 command = twe.getc();
oichan 6:aae92602ef84 59 if(_input(command)==-1){ //フライトモードへ移行
oichan 3:71a45bae8a37 60 oshirase1 = 0;
oichan 3:71a45bae8a37 61 oshirase2 = 1;
oichan 4:4920f8106896 62 _flight();
oichan 4:4920f8106896 63 break;
oichan 6:aae92602ef84 64 }else if(_input(command)==1){ //サーボ動かすだけモードへ
oichan 3:71a45bae8a37 65 oshirase1 = 1;
oichan 3:71a45bae8a37 66 oshirase2 = 0;
oichan 3:71a45bae8a37 67 while(1){
oichan 6:aae92602ef84 68 command = twe.getc();
oichan 6:aae92602ef84 69 if(_input(command)==3){ //開放用サーボ(MG996)を動かす
oichan 3:71a45bae8a37 70 oshirase1 = 1;
oichan 3:71a45bae8a37 71 oshirase2 = 1;
oichan 3:71a45bae8a37 72 break;
oichan 6:aae92602ef84 73 }else if(_input(command)==7){ //リーフィングのサーボ(SG92)を動かす
oichan 3:71a45bae8a37 74 oshirase1 = 0;
oichan 3:71a45bae8a37 75 oshirase2 = 0;
oichan 3:71a45bae8a37 76 break;
oichan 3:71a45bae8a37 77 }
oichan 3:71a45bae8a37 78 }
oichan 3:71a45bae8a37 79 }
oichan 3:71a45bae8a37 80 }
oichan 4:4920f8106896 81 while(1);
oichan 2:0cf17bba70ec 82 }
oichan 2:0cf17bba70ec 83
oichan 2:0cf17bba70ec 84
oichan 2:0cf17bba70ec 85 /* フライトモード用関数 */
oichan 2:0cf17bba70ec 86 void _flight(){
oichan 4:4920f8106896 87 oshirase2.input();
oichan 6:aae92602ef84 88 twe.printf("flight mode on\r\n");
oichan 6:aae92602ef84 89 // pc.printf("flight mode on\r\n");
oichan 2:0cf17bba70ec 90
oichan 2:0cf17bba70ec 91 /* 地上高度取得 */
oichan 2:0cf17bba70ec 92 for(int i=0;i<10;i++){
oichan 2:0cf17bba70ec 93 bmp.ReadData(&Temperature,&Pressure);
oichan 2:0cf17bba70ec 94 Alt_buff[i] = _getAlt(Pressure,Temperature);
oichan 2:0cf17bba70ec 95 }
oichan 2:0cf17bba70ec 96 Land_Alt = _median(Alt_buff,10);
oichan 3:71a45bae8a37 97 Max_Alt = Land_Alt;
oichan 2:0cf17bba70ec 98 FILE *lfp = fopen("/local/alt.txt","a");
oichan 2:0cf17bba70ec 99 fprintf(lfp,"地上高度：%f\r\n",Land_Alt);
oichan 2:0cf17bba70ec 100 fclose(lfp);
oichan 6:aae92602ef84 101 twe.printf("地上高度：%f\r\n",Land_Alt);
oichan 6:aae92602ef84 102 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
mbed_official 0:bdbd3d6fc5d5 103
oichan 2:0cf17bba70ec 104 /* データ取得開始 */
oichan 2:0cf17bba70ec 105 timer.start();
oichan 2:0cf17bba70ec 106 logtimer.attach(_log,1.0/RATE);
oichan 2:0cf17bba70ec 107
oichan 2:0cf17bba70ec 108 while(1){
oichan 6:aae92602ef84 109
oichan 4:4920f8106896 110 /* フェーズ管理 */
oichan 4:4920f8106896 111 if(Phase==0){
oichan 4:4920f8106896 112 if(oshirase2==1){
oichan 5:1e66d892109b 113 Phase = 1; //上昇フェーズへ移行
oichan 4:4920f8106896 114 alt_timer.start();
oichan 4:4920f8106896 115 }
oichan 4:4920f8106896 116 }else if(Phase==1){
oichan 4:4920f8106896 117 if(oshirase2==0){
oichan 5:1e66d892109b 118 Phase = 2; //降下1フェーズへ移行
oichan 4:4920f8106896 119 }
oichan 4:4920f8106896 120 }else if(Phase==2){
oichan 4:4920f8106896 121 if(oshirase2==1){
oichan 5:1e66d892109b 122 Phase = 3; //降下2フェーズへ移行
oichan 4:4920f8106896 123 }
oichan 4:4920f8106896 124 }
oichan 6:aae92602ef84 125
oichan 6:aae92602ef84 126 /* 30分ごとにタイマーをリセット(オーバーフロー対策) */
oichan 6:aae92602ef84 127 if(Time > 60*30){
oichan 6:aae92602ef84 128 timer.reset();
oichan 6:aae92602ef84 129 }
oichan 4:4920f8106896 130
oichan 4:4920f8106896 131 /* GPS取得&送信 */
oichan 2:0cf17bba70ec 132 gps_data[cnt_gps] = gps.getc();
oichan 2:0cf17bba70ec 133 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
oichan 2:0cf17bba70ec 134 cnt_gps = 0;
oichan 2:0cf17bba70ec 135 memset(gps_data,'\0',256);
oichan 2:0cf17bba70ec 136 }else if(gps_data[cnt_gps] == '\r'){
oichan 2:0cf17bba70ec 137 float world_time, lon_east, lat_north;
oichan 2:0cf17bba70ec 138 int rlock, sat_num;
oichan 2:0cf17bba70ec 139 char lat,lon;
oichan 2:0cf17bba70ec 140 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 141 if(rlock==1){
oichan 2:0cf17bba70ec 142 lat_north = _DMS2DEG(lat_north);
oichan 2:0cf17bba70ec 143 lon_east = _DMS2DEG(lon_east);
oichan 4:4920f8106896 144 twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
oichan 2:0cf17bba70ec 145 twe.printf("%s\r\n",gps_data);
oichan 2:0cf17bba70ec 146 twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
oichan 2:0cf17bba70ec 147 }else{
oichan 6:aae92602ef84 148 twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
oichan 2:0cf17bba70ec 149 twe.printf("%s\r\n",gps_data);
oichan 2:0cf17bba70ec 150 }
oichan 2:0cf17bba70ec 151 }
oichan 2:0cf17bba70ec 152 }else{
oichan 2:0cf17bba70ec 153 cnt_gps++;
oichan 2:0cf17bba70ec 154 }
oichan 2:0cf17bba70ec 155 }
oichan 2:0cf17bba70ec 156 }
oichan 2:0cf17bba70ec 157
oichan 2:0cf17bba70ec 158
oichan 2:0cf17bba70ec 159 /* データ取得&保存関数 */
oichan 2:0cf17bba70ec 160 void _log(){
oichan 2:0cf17bba70ec 161 Time = timer.read();
oichan 2:0cf17bba70ec 162 mpu.getAccelero(a);
oichan 2:0cf17bba70ec 163 bmp.ReadData(&Temperature,&Pressure);
oichan 2:0cf17bba70ec 164 Altitude = _getAlt(Pressure,Temperature);
oichan 4:4920f8106896 165
oichan 4:4920f8106896 166 /* 最高高度取得＆保存 */
oichan 2:0cf17bba70ec 167 if(Altitude > Max_Alt){
oichan 4:4920f8106896 168 Max_Alt = Altitude;
oichan 4:4920f8106896 169 alt_timer.reset();
oichan 6:aae92602ef84 170 }else if(alt_timer.read() > MAX_JUDGE_TIME_s){
oichan 4:4920f8106896 171 alt_timer.stop();
oichan 2:0cf17bba70ec 172 alt_timer.reset();
oichan 2:0cf17bba70ec 173 FILE *lfp = fopen("/local/alt.txt","a");
oichan 2:0cf17bba70ec 174 fprintf(lfp,"最高高度：%f\r\n",Max_Alt);
oichan 2:0cf17bba70ec 175 fclose(lfp);
oichan 6:aae92602ef84 176 // pc.printf("最高高度：%f\r\n",Max_Alt);
oichan 2:0cf17bba70ec 177 }
oichan 4:4920f8106896 178
oichan 4:4920f8106896 179 /* データ保存 */
oichan 2:0cf17bba70ec 180 if(Log_cnt==0) {
oichan 7:5f8080485160 181 fp = fopen("/sd/mydir/log.txt", "a");
oichan 2:0cf17bba70ec 182 }
oichan 4:4920f8106896 183 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 184 Log_cnt++;
oichan 2:0cf17bba70ec 185 if(Log_cnt==RATE){
oichan 2:0cf17bba70ec 186 fclose(fp);
oichan 2:0cf17bba70ec 187 Log_cnt = 0;
oichan 2:0cf17bba70ec 188 }
oichan 6:aae92602ef84 189 // 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 190 }
oichan 2:0cf17bba70ec 191
oichan 2:0cf17bba70ec 192
oichan 2:0cf17bba70ec 193 /* 高度計算関数 */
oichan 2:0cf17bba70ec 194 float _getAlt(float press, float temp){
oichan 2:0cf17bba70ec 195 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
mbed_official 0:bdbd3d6fc5d5 196 }
oichan 2:0cf17bba70ec 197
oichan 2:0cf17bba70ec 198
oichan 3:71a45bae8a37 199 /* 緯度経度計算関数 */
oichan 2:0cf17bba70ec 200 float _DMS2DEG(float raw_data){
oichan 2:0cf17bba70ec 201 int d = (int)(raw_data/100);
oichan 2:0cf17bba70ec 202 float m = (raw_data - (float)d*100);
oichan 2:0cf17bba70ec 203 return (float)d + m/60;
oichan 2:0cf17bba70ec 204 }
oichan 2:0cf17bba70ec 205
oichan 2:0cf17bba70ec 206
oichan 2:0cf17bba70ec 207 /* 中央値計算関数 */
oichan 2:0cf17bba70ec 208 float _median(float data[], int num){//todo:処理時間計測
oichan 2:0cf17bba70ec 209 float *data_cpy, ans;
oichan 2:0cf17bba70ec 210 data_cpy = new float[num];
oichan 2:0cf17bba70ec 211 memcpy(data_cpy,data,sizeof(float)*num);
oichan 2:0cf17bba70ec 212
oichan 2:0cf17bba70ec 213 for(int i=0; i<num; i++){
oichan 2:0cf17bba70ec 214 for(int j=0; j<num-i-1; j++){
oichan 2:0cf17bba70ec 215 if(data_cpy[j]>data_cpy[j+1]){
oichan 2:0cf17bba70ec 216 float buff = data_cpy[j+1];
oichan 2:0cf17bba70ec 217 data_cpy[j+1] = data_cpy[j];
oichan 2:0cf17bba70ec 218 data_cpy[j] = buff;
oichan 2:0cf17bba70ec 219 }
oichan 2:0cf17bba70ec 220 }
oichan 2:0cf17bba70ec 221 }
oichan 2:0cf17bba70ec 222
oichan 2:0cf17bba70ec 223 if(num%2!=0) ans = data_cpy[num/2];
oichan 2:0cf17bba70ec 224 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
oichan 2:0cf17bba70ec 225 delete[] data_cpy;
oichan 2:0cf17bba70ec 226 return ans;
oichan 2:0cf17bba70ec 227 }
oichan 3:71a45bae8a37 228
oichan 6:aae92602ef84 229 /* コマンド判別用関数 */
oichan 6:aae92602ef84 230 int _input(char moji){
oichan 6:aae92602ef84 231 if(moji!='\0'){
oichan 6:aae92602ef84 232 twe.printf("%c\r\n",c);
oichan 3:71a45bae8a37 233 }
oichan 6:aae92602ef84 234 if(moji=='f'){
oichan 3:71a45bae8a37 235 return -1;
oichan 6:aae92602ef84 236 }else if(moji=='s'){
oichan 4:4920f8106896 237 return 1;
oichan 6:aae92602ef84 238 }else if(moji=='p'){
oichan 4:4920f8106896 239 return 3;
oichan 6:aae92602ef84 240 }else if(moji=='l'){
oichan 4:4920f8106896 241 return 7;
oichan 3:71a45bae8a37 242 }
oichan 3:71a45bae8a37 243 return 0;
oichan 5:1e66d892109b 244 }