kohei oikawa / Mbed 2 deprecated 2017_11_1768

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

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

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 
flight model

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 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 }