kohei oikawa / Mbed 2 deprecated 2017_11_1768

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

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

main.cpp@4:4920f8106896, 2017-10-22 (annotated)

Committer:
oichan
Date:
Sun Oct 22 04:37:12 2017 +0000
Revision:
4:4920f8106896
Parent:
3:71a45bae8a37
Child:
5:1e66d892109b
a little change

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