CORE / Mbed 2 deprecated 2017_11_1768

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

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of 2017_11_1768 by kohei oikawa

main.cpp@3:71a45bae8a37, 2017-10-21 (annotated)

Committer:
oichan
Date:
Sat Oct 21 16:11:16 2017 +0000
Revision:
3:71a45bae8a37
Parent:
2:0cf17bba70ec
Child:
4:4920f8106896
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 2:0cf17bba70ec 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 2:0cf17bba70ec 42 int cnt_gps;
oichan 2:0cf17bba70ec 43 int Log_cnt=0;
oichan 2:0cf17bba70ec 44
oichan 2:0cf17bba70ec 45
mbed_official 0:bdbd3d6fc5d5 46 int main() {
oichan 2:0cf17bba70ec 47 printf("Hello World!\n");
oichan 2:0cf17bba70ec 48 twe.baud(115200);
oichan 2:0cf17bba70ec 49 mpu.setAcceleroRange(0);
oichan 2:0cf17bba70ec 50 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
oichan 3:71a45bae8a37 51 oshirase2.output();
mbed_official 0:bdbd3d6fc5d5 52 mkdir("/sd/mydir", 0777);
oichan 3:71a45bae8a37 53 oshirase1 = 0;
oichan 3:71a45bae8a37 54 oshirase2 = 0;
oichan 3:71a45bae8a37 55 /* while(1){
oichan 3:71a45bae8a37 56 if(_input(c)==-1){
oichan 3:71a45bae8a37 57 oshirase1 = 0;
oichan 3:71a45bae8a37 58 oshirase2 = 1;
oichan 3:71a45bae8a37 59 */ _flight();
oichan 3:71a45bae8a37 60 /* break;
oichan 3:71a45bae8a37 61 }else if(_input(c)==1){
oichan 3:71a45bae8a37 62 oshirase1 = 1;
oichan 3:71a45bae8a37 63 oshirase2 = 0;
oichan 3:71a45bae8a37 64 while(1){
oichan 3:71a45bae8a37 65 if(_input(c)==3){
oichan 3:71a45bae8a37 66 oshirase1 = 1;
oichan 3:71a45bae8a37 67 oshirase2 = 1;
oichan 3:71a45bae8a37 68 break;
oichan 3:71a45bae8a37 69 }else if(_input(c)==7){
oichan 3:71a45bae8a37 70 oshirase1 = 0;
oichan 3:71a45bae8a37 71 oshirase2 = 0;
oichan 3:71a45bae8a37 72 break;
oichan 3:71a45bae8a37 73 }
oichan 3:71a45bae8a37 74 }
oichan 3:71a45bae8a37 75 }
oichan 3:71a45bae8a37 76 }
oichan 3:71a45bae8a37 77 */ while(1);
oichan 2:0cf17bba70ec 78 }
oichan 2:0cf17bba70ec 79
oichan 2:0cf17bba70ec 80
oichan 2:0cf17bba70ec 81 /* フライトモード用関数 */
oichan 2:0cf17bba70ec 82 void _flight(){
oichan 2:0cf17bba70ec 83
oichan 2:0cf17bba70ec 84 /* 地上高度取得 */
oichan 2:0cf17bba70ec 85 for(int i=0;i<10;i++){
oichan 2:0cf17bba70ec 86 bmp.ReadData(&Temperature,&Pressure);
oichan 2:0cf17bba70ec 87 Alt_buff[i] = _getAlt(Pressure,Temperature);
oichan 2:0cf17bba70ec 88 }
oichan 2:0cf17bba70ec 89 Land_Alt = _median(Alt_buff,10);
oichan 3:71a45bae8a37 90 Max_Alt = Land_Alt;
oichan 2:0cf17bba70ec 91 FILE *lfp = fopen("/local/alt.txt","a");
oichan 2:0cf17bba70ec 92 fprintf(lfp,"地上高度：%f\r\n",Land_Alt);
oichan 2:0cf17bba70ec 93 fclose(lfp);
oichan 2:0cf17bba70ec 94 for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
mbed_official 0:bdbd3d6fc5d5 95
oichan 2:0cf17bba70ec 96 /* データ取得開始 */
oichan 2:0cf17bba70ec 97 timer.start();
oichan 2:0cf17bba70ec 98 logtimer.attach(_log,1.0/RATE);
oichan 3:71a45bae8a37 99 alt_timer.start(); //発射判定後にスタートするようにする！！
oichan 2:0cf17bba70ec 100
oichan 2:0cf17bba70ec 101 /* GPS取得&送信 */
oichan 2:0cf17bba70ec 102 while(1){
oichan 2:0cf17bba70ec 103 gps_data[cnt_gps] = gps.getc();
oichan 2:0cf17bba70ec 104 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
oichan 2:0cf17bba70ec 105 cnt_gps = 0;
oichan 2:0cf17bba70ec 106 memset(gps_data,'\0',256);
oichan 2:0cf17bba70ec 107 }else if(gps_data[cnt_gps] == '\r'){
oichan 2:0cf17bba70ec 108 float world_time, lon_east, lat_north;
oichan 2:0cf17bba70ec 109 int rlock, sat_num;
oichan 2:0cf17bba70ec 110 char lat,lon;
oichan 2:0cf17bba70ec 111 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 112 if(rlock==1){
oichan 2:0cf17bba70ec 113 lat_north = _DMS2DEG(lat_north);
oichan 2:0cf17bba70ec 114 lon_east = _DMS2DEG(lon_east);
oichan 2:0cf17bba70ec 115 twe.printf("max altitude:%f\r\n",Max_Alt);
oichan 2:0cf17bba70ec 116 twe.printf("%s\r\n",gps_data);
oichan 2:0cf17bba70ec 117 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 118 }else{
oichan 2:0cf17bba70ec 119 twe.printf("max altitude:%f\r\n",Max_Alt);
oichan 2:0cf17bba70ec 120 twe.printf("%s\r\n",gps_data);
oichan 2:0cf17bba70ec 121 }
oichan 2:0cf17bba70ec 122 }
oichan 2:0cf17bba70ec 123 }else{
oichan 2:0cf17bba70ec 124 cnt_gps++;
oichan 2:0cf17bba70ec 125 }
oichan 2:0cf17bba70ec 126 }
oichan 2:0cf17bba70ec 127 }
oichan 2:0cf17bba70ec 128
oichan 2:0cf17bba70ec 129
oichan 2:0cf17bba70ec 130 /* データ取得&保存関数 */
oichan 2:0cf17bba70ec 131 void _log(){
oichan 2:0cf17bba70ec 132 Time = timer.read();
oichan 2:0cf17bba70ec 133 mpu.getAccelero(a);
oichan 2:0cf17bba70ec 134 bmp.ReadData(&Temperature,&Pressure);
oichan 2:0cf17bba70ec 135 Altitude = _getAlt(Pressure,Temperature);
oichan 3:71a45bae8a37 136 //TODO:発射判定後に行うようにする！！(ここから)
oichan 2:0cf17bba70ec 137 if(Altitude > Max_Alt){
oichan 2:0cf17bba70ec 138 Max_Alt = Altitude;
oichan 2:0cf17bba70ec 139 alt_timer.reset();
mbed_official 0:bdbd3d6fc5d5 140 }
oichan 2:0cf17bba70ec 141 if(alt_timer.read() > MAX_JUDGE_TIME_s){
oichan 2:0cf17bba70ec 142 alt_timer.reset();
oichan 2:0cf17bba70ec 143 alt_timer.stop();
oichan 2:0cf17bba70ec 144 FILE *lfp = fopen("/local/alt.txt","a");
oichan 2:0cf17bba70ec 145 fprintf(lfp,"最高高度：%f\r\n",Max_Alt);
oichan 2:0cf17bba70ec 146 fclose(lfp);
oichan 2:0cf17bba70ec 147 }
oichan 3:71a45bae8a37 148 //TODO:発射判定後に行うようにする！！(ここまで)
oichan 2:0cf17bba70ec 149 if(Log_cnt==0) {
oichan 2:0cf17bba70ec 150 fp = fopen("/sd/mydir/sdtest.txt", "a");
oichan 2:0cf17bba70ec 151 }
oichan 2:0cf17bba70ec 152 fprintf(fp, "%f, %f, %f, %f, %f, %f, %f \r\n",Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
oichan 2:0cf17bba70ec 153 Log_cnt++;
oichan 2:0cf17bba70ec 154 if(Log_cnt==RATE){
oichan 2:0cf17bba70ec 155 fclose(fp);
oichan 2:0cf17bba70ec 156 Log_cnt = 0;
oichan 2:0cf17bba70ec 157 }
oichan 2:0cf17bba70ec 158 pc.printf("%f, %f, %f, %f, %f, %f, %f \r\n",Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
oichan 2:0cf17bba70ec 159 }
oichan 2:0cf17bba70ec 160
oichan 2:0cf17bba70ec 161
oichan 2:0cf17bba70ec 162 /* 高度計算関数 */
oichan 2:0cf17bba70ec 163 float _getAlt(float press, float temp){
oichan 2:0cf17bba70ec 164 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
mbed_official 0:bdbd3d6fc5d5 165 }
oichan 2:0cf17bba70ec 166
oichan 2:0cf17bba70ec 167
oichan 3:71a45bae8a37 168 /* 緯度経度計算関数 */
oichan 2:0cf17bba70ec 169 float _DMS2DEG(float raw_data){
oichan 2:0cf17bba70ec 170 int d = (int)(raw_data/100);
oichan 2:0cf17bba70ec 171 float m = (raw_data - (float)d*100);
oichan 2:0cf17bba70ec 172 return (float)d + m/60;
oichan 2:0cf17bba70ec 173 }
oichan 2:0cf17bba70ec 174
oichan 2:0cf17bba70ec 175
oichan 2:0cf17bba70ec 176 /* 中央値計算関数 */
oichan 2:0cf17bba70ec 177 float _median(float data[], int num){//todo:処理時間計測
oichan 2:0cf17bba70ec 178 float *data_cpy, ans;
oichan 2:0cf17bba70ec 179 data_cpy = new float[num];
oichan 2:0cf17bba70ec 180 memcpy(data_cpy,data,sizeof(float)*num);
oichan 2:0cf17bba70ec 181
oichan 2:0cf17bba70ec 182 for(int i=0; i<num; i++){
oichan 2:0cf17bba70ec 183 for(int j=0; j<num-i-1; j++){
oichan 2:0cf17bba70ec 184 if(data_cpy[j]>data_cpy[j+1]){
oichan 2:0cf17bba70ec 185 float buff = data_cpy[j+1];
oichan 2:0cf17bba70ec 186 data_cpy[j+1] = data_cpy[j];
oichan 2:0cf17bba70ec 187 data_cpy[j] = buff;
oichan 2:0cf17bba70ec 188 }
oichan 2:0cf17bba70ec 189 }
oichan 2:0cf17bba70ec 190 }
oichan 2:0cf17bba70ec 191
oichan 2:0cf17bba70ec 192 if(num%2!=0) ans = data_cpy[num/2];
oichan 2:0cf17bba70ec 193 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
oichan 2:0cf17bba70ec 194 delete[] data_cpy;
oichan 2:0cf17bba70ec 195 return ans;
oichan 2:0cf17bba70ec 196 }
oichan 3:71a45bae8a37 197
oichan 3:71a45bae8a37 198 int _input(char c){
oichan 3:71a45bae8a37 199 if(c!='\0'){
oichan 3:71a45bae8a37 200 // pc.printf("%c\r\n",c);
oichan 3:71a45bae8a37 201 }
oichan 3:71a45bae8a37 202 if(c=='f'){
oichan 3:71a45bae8a37 203 // pc.printf("flight mode on\r\n");
oichan 3:71a45bae8a37 204 return -1;
oichan 3:71a45bae8a37 205 }
oichan 3:71a45bae8a37 206 return 0;
oichan 3:71a45bae8a37 207 }