Haruki Sashida / Mbed 2 deprecated 201803_oshima_gedan

201803_oshima Team.F.C.

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of 201803_oshima_jodan by Haruki Sashida

main.cpp@8:eb1ebae5f051, 2018-03-06 (annotated)

Committer:
sashida_h
Date:
Tue Mar 06 13:04:18 2018 +0000
Revision:
8:eb1ebae5f051
Parent:
7:852922a4058a
Child:
9:b10aaf72d100
I'm fine , thank you.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:bdbd3d6fc5d5 1 #include "mbed.h"
sashida_h 5:bb9c685fc1fe 2 #include "SDFileSystem.h"
sashida_h 5:bb9c685fc1fe 3 #include "MPU6050.h"
sashida_h 5:bb9c685fc1fe 4 #include "math.h"
sashida_h 2:0deade364b73 5 #include "BMP180.h"
sashida_h 2:0deade364b73 6
sashida_h 2:0deade364b73 7 #define UNLOCK 1
sashida_h 2:0deade364b73 8 #define LOCK 0
sashida_h 3:a7b39e55d100 9 #define TIMER 30 //開放タイマー
sashida_h 5:bb9c685fc1fe 10 #define RATE 50//判定周期
mbed_official 0:bdbd3d6fc5d5 11
sashida_h 2:0deade364b73 12 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
sashida_h 2:0deade364b73 13 BMP180 bmp(p28,p27);
sashida_h 2:0deade364b73 14 MPU6050 mpu(p28,p27);
sashida_h 5:bb9c685fc1fe 15 DigitalIn fly(p12);
sashida_h 5:bb9c685fc1fe 16 DigitalOut myled(p20);
sashida_h 5:bb9c685fc1fe 17 DigitalOut myled2(p19);
sashida_h 5:bb9c685fc1fe 18 DigitalOut myled3(p18);
sashida_h 5:bb9c685fc1fe 19 DigitalOut myled4(LED1);
sashida_h 5:bb9c685fc1fe 20 DigitalOut myled5(LED2);
sashida_h 5:bb9c685fc1fe 21 DigitalOut myled6(LED3);
sashida_h 5:bb9c685fc1fe 22 DigitalOut myled7(LED4);
sashida_h 2:0deade364b73 23 Serial twe(p9,p10);
sashida_h 2:0deade364b73 24 Serial gps(p13,p14);
sashida_h 2:0deade364b73 25 Serial pc(USBTX,USBRX);
sashida_h 5:bb9c685fc1fe 26 PwmOut servo_para(p23);
sashida_h 2:0deade364b73 27
sashida_h 2:0deade364b73 28 LocalFileSystem local("local");
sashida_h 2:0deade364b73 29 FILE *fp;
sashida_h 2:0deade364b73 30 FILE *lfp;
sashida_h 2:0deade364b73 31
sashida_h 2:0deade364b73 32 Ticker kaihou;
sashida_h 5:bb9c685fc1fe 33 Ticker ochiru;
sashida_h 2:0deade364b73 34 Timer timer1;
sashida_h 5:bb9c685fc1fe 35
sashida_h 3:a7b39e55d100 36 /*とりあえず全てグローバル変数化してます。*/
sashida_h 2:0deade364b73 37 //カウント変数
sashida_h 2:0deade364b73 38 int Cnt_buff = 0;
sashida_h 2:0deade364b73 39 int Cnt_para = 0;
sashida_h 2:0deade364b73 40 int Cnt_open = 0;
sashida_h 8:eb1ebae5f051 41 int i, j, t;
sashida_h 2:0deade364b73 42 //高度取得
sashida_h 5:bb9c685fc1fe 43 float Alt_buff[256],Alt_buff2[10],Alt_gnd,Alt_now, Max_Alt;
sashida_h 2:0deade364b73 44 float p0 = 1013.25; //海面気圧
sashida_h 2:0deade364b73 45 float pressure,temperature, Alt;
sashida_h 2:0deade364b73 46 //位置情報取得
sashida_h 2:0deade364b73 47 char gps_data[256];
sashida_h 2:0deade364b73 48 int cnt_gps =0;
sashida_h 8:eb1ebae5f051 49 //加速度,ジャイロ取得
sashida_h 2:0deade364b73 50 float a_abs;
sashida_h 2:0deade364b73 51 float a[3];
sashida_h 2:0deade364b73 52 float d[3];
sashida_h 2:0deade364b73 53
sashida_h 2:0deade364b73 54 bool tf_para = true;
sashida_h 5:bb9c685fc1fe 55
sashida_h 2:0deade364b73 56 float median(float data[], int num); //中央値求める
sashida_h 2:0deade364b73 57 float get_Alt(float press, float temp);
sashida_h 2:0deade364b73 58 void _open(void); //kaihou
sashida_h 2:0deade364b73 59 float _DMS2DEG(float raw_data); //GPSデータ60進数→10進数
sashida_h 2:0deade364b73 60 void _para(int motion);
sashida_h 5:bb9c685fc1fe 61 void _recovery();
sashida_h 5:bb9c685fc1fe 62
sashida_h 2:0deade364b73 63 int main() {
sashida_h 5:bb9c685fc1fe 64 twe.baud(115200);
sashida_h 5:bb9c685fc1fe 65 twe.printf("Hello World!\r\n");
sashida_h 2:0deade364b73 66
sashida_h 5:bb9c685fc1fe 67 _para(LOCK);
sashida_h 2:0deade364b73 68
sashida_h 2:0deade364b73 69 mpu.setAcceleroRange(0);
sashida_h 2:0deade364b73 70 mpu.setGyroRange(0);
sashida_h 5:bb9c685fc1fe 71 bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
sashida_h 2:0deade364b73 72 twe.printf("I2C_initialize_ok\r\n");
sashida_h 5:bb9c685fc1fe 73
sashida_h 3:a7b39e55d100 74
sashida_h 5:bb9c685fc1fe 75 //地上高度取得
sashida_h 5:bb9c685fc1fe 76 Cnt_buff = 0;
sashida_h 2:0deade364b73 77 for(i=0; i<10; i++){
sashida_h 2:0deade364b73 78 bmp.ReadData(&temperature,&pressure);
sashida_h 2:0deade364b73 79 Alt_buff[i]=get_Alt(pressure, temperature);
sashida_h 2:0deade364b73 80 }
sashida_h 2:0deade364b73 81 Alt_gnd = median(Alt_buff, 10);
sashida_h 2:0deade364b73 82
sashida_h 5:bb9c685fc1fe 83 twe.printf("Alt_gnd:%f\r\n", Alt_gnd);
sashida_h 5:bb9c685fc1fe 84
sashida_h 5:bb9c685fc1fe 85 lfp = fopen("/local/Alt.txt","a");
sashida_h 5:bb9c685fc1fe 86 fprintf(lfp, "GND:%f\r\n",Alt_gnd);
sashida_h 5:bb9c685fc1fe 87 fclose(lfp);
sashida_h 5:bb9c685fc1fe 88 twe.printf("lacal_ok\r\n");
sashida_h 5:bb9c685fc1fe 89
sashida_h 5:bb9c685fc1fe 90 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 2:0deade364b73 91 if(fp == NULL) {
sashida_h 2:0deade364b73 92 error("Could not open file for write\n");
sashida_h 2:0deade364b73 93 }
sashida_h 2:0deade364b73 94 for(i=0; i<10; i++){
sashida_h 2:0deade364b73 95 fprintf(fp, "%d:%f\r\n",i+1,Alt_buff[i]);
sashida_h 2:0deade364b73 96 }
sashida_h 2:0deade364b73 97
sashida_h 2:0deade364b73 98 fprintf(fp, "Alt_gnd:%f\r\n",Alt_gnd);
sashida_h 5:bb9c685fc1fe 99 fclose(fp);
sashida_h 2:0deade364b73 100
sashida_h 5:bb9c685fc1fe 101 twe.printf("SD_write_OK!!\r\n");
sashida_h 5:bb9c685fc1fe 102
sashida_h 5:bb9c685fc1fe 103 twe.printf("ALL_READY\r\n");
sashida_h 2:0deade364b73 104
sashida_h 2:0deade364b73 105 while(fly == 1); //フライトピン抜けるまで待機
sashida_h 2:0deade364b73 106
sashida_h 5:bb9c685fc1fe 107 i = 0;
sashida_h 5:bb9c685fc1fe 108 Alt_buff[0] = 0;
sashida_h 5:bb9c685fc1fe 109 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 5:bb9c685fc1fe 110 if(fp == NULL)twe.printf("ERROR\r\n");
sashida_h 2:0deade364b73 111 timer1.start();
sashida_h 8:eb1ebae5f051 112 kaihou.attach(_open,1/RATE);
sashida_h 8:eb1ebae5f051 113 kaihou.attach(_open,0.02);
sashida_h 5:bb9c685fc1fe 114
sashida_h 2:0deade364b73 115
sashida_h 2:0deade364b73 116 while(1){
sashida_h 2:0deade364b73 117
sashida_h 5:bb9c685fc1fe 118 /* twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para);
sashida_h 5:bb9c685fc1fe 119 wait(1.0);
sashida_h 5:bb9c685fc1fe 120 */
sashida_h 2:0deade364b73 121 /* GPS取得&送信 */
sashida_h 5:bb9c685fc1fe 122 gps_data[cnt_gps] = gps.getc();
sashida_h 2:0deade364b73 123 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 2:0deade364b73 124 cnt_gps = 0;
sashida_h 2:0deade364b73 125 memset(gps_data,'\0',256);
sashida_h 2:0deade364b73 126 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 2:0deade364b73 127 float world_time, lon_east, lat_north;
sashida_h 2:0deade364b73 128 int rlock, sat_num;
sashida_h 2:0deade364b73 129 char lat,lon;
sashida_h 2:0deade364b73 130 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
sashida_h 2:0deade364b73 131 if(rlock==1){
sashida_h 2:0deade364b73 132 lat_north = _DMS2DEG(lat_north);
sashida_h 2:0deade364b73 133 lon_east = _DMS2DEG(lon_east);
sashida_h 2:0deade364b73 134 // pc.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
sashida_h 2:0deade364b73 135 // twe.printf("%s\r\n",gps_data);
sashida_h 2:0deade364b73 136 twe.printf("Lat,Lon:%f,%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
sashida_h 2:0deade364b73 137 }else{
sashida_h 2:0deade364b73 138 // pc.printf("max altitude:%f\r\n",Max_Alt);
sashida_h 2:0deade364b73 139 twe.printf("%s\r\n",gps_data);
sashida_h 2:0deade364b73 140 }
sashida_h 5:bb9c685fc1fe 141 twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt,Cnt_para);
sashida_h 2:0deade364b73 142 }
sashida_h 2:0deade364b73 143 }else{
sashida_h 2:0deade364b73 144 cnt_gps++;
sashida_h 2:0deade364b73 145 }
sashida_h 2:0deade364b73 146
sashida_h 5:bb9c685fc1fe 147 }//while_gps
sashida_h 5:bb9c685fc1fe 148 }//main
sashida_h 5:bb9c685fc1fe 149
sashida_h 5:bb9c685fc1fe 150 void _open(){
sashida_h 5:bb9c685fc1fe 151 bmp.ReadData(&temperature,&pressure);
sashida_h 5:bb9c685fc1fe 152 mpu.getAccelero(a);
sashida_h 5:bb9c685fc1fe 153 mpu.getGyro(d);
sashida_h 5:bb9c685fc1fe 154 Alt_buff2[i] = get_Alt(pressure, temperature);
sashida_h 5:bb9c685fc1fe 155 i++;
sashida_h 6:e9004c78b394 156 if(i == 10){ //10回ごとに中央値計算
sashida_h 5:bb9c685fc1fe 157 Alt_now = median(Alt_buff2, 10);
sashida_h 5:bb9c685fc1fe 158 Alt_now = Alt_now - Alt_gnd;
sashida_h 5:bb9c685fc1fe 159 t = timer1.read();
sashida_h 5:bb9c685fc1fe 160 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%d,%d\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2],t,Cnt_para);
sashida_h 5:bb9c685fc1fe 161 if(Alt_now > Max_Alt) Max_Alt = Alt_now;
sashida_h 5:bb9c685fc1fe 162 i = 0;
sashida_h 6:e9004c78b394 163 if(tf_para == true){ //パラ開くまでは頂点判定
sashida_h 5:bb9c685fc1fe 164 Alt_buff[Cnt_buff+1] = Alt_now;
sashida_h 5:bb9c685fc1fe 165 if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; //直前の値より小さければカウント+1
sashida_h 5:bb9c685fc1fe 166 // twe.printf("Cnt_para:%d\r\n", Cnt_para);
sashida_h 5:bb9c685fc1fe 167 Cnt_buff++;
sashida_h 6:e9004c78b394 168 if(Cnt_para == 10 || t > TIMER){ //頂点！！！
sashida_h 6:e9004c78b394 169 kaihou.detach(); //SD閉じる前にサーボ動かす前にTicker止める
sashida_h 6:e9004c78b394 170 fprintf(fp,"OPEN!\r\n");
sashida_h 5:bb9c685fc1fe 171 fclose(fp);
sashida_h 5:bb9c685fc1fe 172 _para(UNLOCK);
sashida_h 5:bb9c685fc1fe 173 tf_para = false;
sashida_h 5:bb9c685fc1fe 174 timer1.stop();
sashida_h 7:852922a4058a 175 twe.printf("PARA_OPEN\r\n");
sashida_h 7:852922a4058a 176 i = 0;
sashida_h 6:e9004c78b394 177 ochiru.attach(_recovery,0.1); //パラメータ保存のためのやつ
sashida_h 5:bb9c685fc1fe 178
sashida_h 5:bb9c685fc1fe 179 }
sashida_h 5:bb9c685fc1fe 180 }
sashida_h 2:0deade364b73 181 }
sashida_h 7:852922a4058a 182
sashida_h 7:852922a4058a 183 if(Cnt_buff == 250) Cnt_buff = 0;
sashida_h 5:bb9c685fc1fe 184
mbed_official 0:bdbd3d6fc5d5 185 }
sashida_h 2:0deade364b73 186
sashida_h 5:bb9c685fc1fe 187 void _recovery(){
sashida_h 2:0deade364b73 188
sashida_h 7:852922a4058a 189 if(i == 0){
sashida_h 2:0deade364b73 190 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 2:0deade364b73 191 if(fp == NULL)twe.printf("ERROR\r\n");
sashida_h 2:0deade364b73 192 }
sashida_h 2:0deade364b73 193
sashida_h 5:bb9c685fc1fe 194 bmp.ReadData(&temperature,&pressure);
sashida_h 5:bb9c685fc1fe 195 mpu.getAccelero(a);
sashida_h 5:bb9c685fc1fe 196 mpu.getGyro(d);
sashida_h 5:bb9c685fc1fe 197 Alt_now = get_Alt(pressure, temperature);
sashida_h 5:bb9c685fc1fe 198 Alt_now = Alt_now - Alt_gnd;
sashida_h 5:bb9c685fc1fe 199 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]);
sashida_h 5:bb9c685fc1fe 200 Cnt_buff++;
sashida_h 2:0deade364b73 201
sashida_h 7:852922a4058a 202 if(i == 50){
sashida_h 5:bb9c685fc1fe 203 fclose(fp);
sashida_h 2:0deade364b73 204 Cnt_buff = 0;
sashida_h 2:0deade364b73 205 }
sashida_h 2:0deade364b73 206 }
sashida_h 2:0deade364b73 207 float get_Alt(float press, float temp){
sashida_h 2:0deade364b73 208 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
sashida_h 2:0deade364b73 209 }
sashida_h 2:0deade364b73 210
sashida_h 2:0deade364b73 211 float median(float data[], int num){//todo:処理時間計測
sashida_h 2:0deade364b73 212 float *data_cpy, ans;
sashida_h 2:0deade364b73 213 data_cpy = new float[num];
sashida_h 2:0deade364b73 214 memcpy(data_cpy,data,sizeof(float)*num);
sashida_h 2:0deade364b73 215
sashida_h 2:0deade364b73 216 for(int i=0; i<num; i++){
sashida_h 2:0deade364b73 217 for(int j=0; j<num-i-1; j++){
sashida_h 2:0deade364b73 218 if(data_cpy[j]>data_cpy[j+1]){
sashida_h 2:0deade364b73 219 float buff = data_cpy[j+1];
sashida_h 2:0deade364b73 220 data_cpy[j+1] = data_cpy[j];
sashida_h 2:0deade364b73 221 data_cpy[j] = buff;
sashida_h 2:0deade364b73 222 }
sashida_h 2:0deade364b73 223 }
sashida_h 2:0deade364b73 224 }
sashida_h 2:0deade364b73 225
sashida_h 2:0deade364b73 226 if(num%2!=0) ans = data_cpy[num/2];
sashida_h 2:0deade364b73 227 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
sashida_h 2:0deade364b73 228 delete[] data_cpy;
sashida_h 2:0deade364b73 229 return ans;
sashida_h 2:0deade364b73 230 }
sashida_h 2:0deade364b73 231
sashida_h 2:0deade364b73 232
sashida_h 2:0deade364b73 233 float _DMS2DEG(float raw_data){
sashida_h 2:0deade364b73 234 int d = (int)(raw_data/100);
sashida_h 2:0deade364b73 235 float m = (raw_data - (float)d*100);
sashida_h 2:0deade364b73 236 return (float)d + m/60;
sashida_h 2:0deade364b73 237 }
sashida_h 2:0deade364b73 238
sashida_h 2:0deade364b73 239 void _para(int motion){
sashida_h 2:0deade364b73 240 if(motion==UNLOCK){
sashida_h 2:0deade364b73 241 servo_para.pulsewidth(0.0005); // pulse servo out sita
sashida_h 2:0deade364b73 242 }else if(motion==LOCK){
sashida_h 2:0deade364b73 243 servo_para.pulsewidth(0.0025); // pulse servo outu sita
sashida_h 2:0deade364b73 244 }
sashida_h 2:0deade364b73 245 }