Haruki Sashida / Mbed 2 deprecated 201803_oshima_jodan

201803_oshima_jodam_test

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

main.cpp@16:22d517bf83a0, 2018-03-24 (annotated)

Committer:
sashida_h
Date:
Sat Mar 24 17:06:29 2018 +0000
Revision:
16:22d517bf83a0
Parent:
15:4ba85a0e8767 
OK

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 15:4ba85a0e8767 9 #define KAIHOU 10 //開放判定開始時間（燃焼時間の間は止める）
sashida_h 3:a7b39e55d100 10 #define TIMER 30 //開放タイマー
sashida_h 15:4ba85a0e8767 11 #define RATE 50.0//判定周期 浮動小数点型で記述してください。
sashida_h 10:273500c77873 12 #define ALT 1 //落下判断高度
mbed_official 0:bdbd3d6fc5d5 13
sashida_h 2:0deade364b73 14 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
sashida_h 2:0deade364b73 15 BMP180 bmp(p28,p27);
sashida_h 2:0deade364b73 16 MPU6050 mpu(p28,p27);
sashida_h 5:bb9c685fc1fe 17 DigitalIn fly(p12);
sashida_h 5:bb9c685fc1fe 18 DigitalOut myled(p20);
sashida_h 5:bb9c685fc1fe 19 DigitalOut myled2(p19);
sashida_h 5:bb9c685fc1fe 20 DigitalOut myled3(p18);
sashida_h 2:0deade364b73 21 Serial twe(p9,p10);
sashida_h 2:0deade364b73 22 Serial gps(p13,p14);
sashida_h 2:0deade364b73 23 Serial pc(USBTX,USBRX);
sashida_h 9:b10aaf72d100 24 PwmOut servo_para(p22);
sashida_h 14:0f267807ce7c 25 PwmOut servo_para2(p21);
sashida_h 2:0deade364b73 26
sashida_h 2:0deade364b73 27 LocalFileSystem local("local");
sashida_h 2:0deade364b73 28 FILE *fp;
sashida_h 2:0deade364b73 29 FILE *lfp;
sashida_h 2:0deade364b73 30
sashida_h 2:0deade364b73 31 Ticker kaihou;
sashida_h 5:bb9c685fc1fe 32 Ticker ochiru;
sashida_h 2:0deade364b73 33 Timer timer1;
sashida_h 5:bb9c685fc1fe 34
sashida_h 3:a7b39e55d100 35 /*とりあえず全てグローバル変数化してます。*/
sashida_h 2:0deade364b73 36 //カウント変数
sashida_h 2:0deade364b73 37 int Cnt_buff = 0;
sashida_h 2:0deade364b73 38 int Cnt_para = 0;
sashida_h 2:0deade364b73 39 int Cnt_open = 0;
sashida_h 8:eb1ebae5f051 40 int i, j, t;
sashida_h 2:0deade364b73 41 //高度取得
sashida_h 5:bb9c685fc1fe 42 float Alt_buff[256],Alt_buff2[10],Alt_gnd,Alt_now, Max_Alt;
sashida_h 2:0deade364b73 43 float p0 = 1013.25; //海面気圧
sashida_h 2:0deade364b73 44 float pressure,temperature, Alt;
sashida_h 2:0deade364b73 45 //位置情報取得
sashida_h 2:0deade364b73 46 char gps_data[256];
sashida_h 2:0deade364b73 47 int cnt_gps =0;
sashida_h 8:eb1ebae5f051 48 //加速度,ジャイロ取得
sashida_h 2:0deade364b73 49 float a_abs;
sashida_h 2:0deade364b73 50 float a[3];
sashida_h 2:0deade364b73 51 float d[3];
sashida_h 14:0f267807ce7c 52 float c[10];
sashida_h 14:0f267807ce7c 53 float Acc_now;
sashida_h 14:0f267807ce7c 54 bool tf_launch = true;
sashida_h 2:0deade364b73 55
sashida_h 2:0deade364b73 56 bool tf_para = true;
sashida_h 5:bb9c685fc1fe 57
sashida_h 2:0deade364b73 58 float median(float data[], int num); //中央値求める
sashida_h 2:0deade364b73 59 float get_Alt(float press, float temp);
sashida_h 2:0deade364b73 60 void _open(void); //kaihou
sashida_h 2:0deade364b73 61 float _DMS2DEG(float raw_data); //GPSデータ60進数→10進数
sashida_h 2:0deade364b73 62 void _para(int motion);
sashida_h 5:bb9c685fc1fe 63 void _recovery();
sashida_h 10:273500c77873 64 int _input(char c);
sashida_h 5:bb9c685fc1fe 65
sashida_h 2:0deade364b73 66 int main() {
sashida_h 5:bb9c685fc1fe 67 twe.baud(115200);
sashida_h 5:bb9c685fc1fe 68 twe.printf("Hello World!\r\n");
sashida_h 2:0deade364b73 69
sashida_h 15:4ba85a0e8767 70 int ret;
sashida_h 11:0f8d26600248 71 lfp = fopen("/local/Alt.txt","r");
sashida_h 11:0f8d26600248 72 if(lfp == NULL){
sashida_h 11:0f8d26600248 73 goto normal;
sashida_h 11:0f8d26600248 74 }
sashida_h 11:0f8d26600248 75 ret = fscanf(lfp,"GND:%f",&Alt_gnd);
sashida_h 11:0f8d26600248 76 fclose(lfp);
sashida_h 11:0f8d26600248 77 if(ret == -1){
sashida_h 11:0f8d26600248 78 goto normal;
sashida_h 11:0f8d26600248 79 }else{
sashida_h 12:c32b14ca0196 80 twe.printf("reboot now\r\n");
sashida_h 11:0f8d26600248 81 goto yabai;
sashida_h 11:0f8d26600248 82 }
sashida_h 11:0f8d26600248 83
sashida_h 11:0f8d26600248 84 normal:
sashida_h 15:4ba85a0e8767 85
sashida_h 10:273500c77873 86 while(1){
sashida_h 10:273500c77873 87 char c = twe.getc();
sashida_h 10:273500c77873 88 if(_input(c) == 1){
sashida_h 10:273500c77873 89 _para(UNLOCK);
sashida_h 10:273500c77873 90 twe.printf("servo_open\r\n");
sashida_h 10:273500c77873 91 }
sashida_h 10:273500c77873 92 else if(_input(c) == 2){
sashida_h 10:273500c77873 93 _para(LOCK);
sashida_h 10:273500c77873 94 twe.printf("servo_lock\r\n");
sashida_h 10:273500c77873 95 }
sashida_h 10:273500c77873 96 else if(_input(c) == 3){
sashida_h 10:273500c77873 97 break;
sashida_h 10:273500c77873 98 }
sashida_h 10:273500c77873 99
sashida_h 10:273500c77873 100 }
sashida_h 16:22d517bf83a0 101
sashida_h 15:4ba85a0e8767 102 yabai:
sashida_h 15:4ba85a0e8767 103
sashida_h 15:4ba85a0e8767 104 mpu.setAcceleroRange(3);
sashida_h 15:4ba85a0e8767 105 mpu.setGyroRange(3);
sashida_h 5:bb9c685fc1fe 106 bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
sashida_h 2:0deade364b73 107 twe.printf("I2C_initialize_ok\r\n");
sashida_h 5:bb9c685fc1fe 108
sashida_h 3:a7b39e55d100 109
sashida_h 5:bb9c685fc1fe 110 //地上高度取得
sashida_h 5:bb9c685fc1fe 111 Cnt_buff = 0;
sashida_h 2:0deade364b73 112 for(i=0; i<10; i++){
sashida_h 2:0deade364b73 113 bmp.ReadData(&temperature,&pressure);
sashida_h 2:0deade364b73 114 Alt_buff[i]=get_Alt(pressure, temperature);
sashida_h 2:0deade364b73 115 }
sashida_h 2:0deade364b73 116 Alt_gnd = median(Alt_buff, 10);
sashida_h 2:0deade364b73 117
sashida_h 5:bb9c685fc1fe 118 twe.printf("Alt_gnd:%f\r\n", Alt_gnd);
sashida_h 5:bb9c685fc1fe 119
sashida_h 5:bb9c685fc1fe 120 lfp = fopen("/local/Alt.txt","a");
sashida_h 5:bb9c685fc1fe 121 fprintf(lfp, "GND:%f\r\n",Alt_gnd);
sashida_h 5:bb9c685fc1fe 122 fclose(lfp);
sashida_h 5:bb9c685fc1fe 123 twe.printf("lacal_ok\r\n");
sashida_h 5:bb9c685fc1fe 124
sashida_h 14:0f267807ce7c 125 mkdir("/sd/mydir", 0777);
sashida_h 15:4ba85a0e8767 126
sashida_h 5:bb9c685fc1fe 127 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 2:0deade364b73 128 if(fp == NULL) {
sashida_h 2:0deade364b73 129 error("Could not open file for write\n");
sashida_h 2:0deade364b73 130 }
sashida_h 2:0deade364b73 131 for(i=0; i<10; i++){
sashida_h 2:0deade364b73 132 fprintf(fp, "%d:%f\r\n",i+1,Alt_buff[i]);
sashida_h 2:0deade364b73 133 }
sashida_h 2:0deade364b73 134
sashida_h 2:0deade364b73 135 fprintf(fp, "Alt_gnd:%f\r\n",Alt_gnd);
sashida_h 5:bb9c685fc1fe 136 fclose(fp);
sashida_h 2:0deade364b73 137
sashida_h 5:bb9c685fc1fe 138 twe.printf("SD_write_OK!!\r\n");
sashida_h 5:bb9c685fc1fe 139
sashida_h 5:bb9c685fc1fe 140 twe.printf("ALL_READY\r\n");
sashida_h 2:0deade364b73 141
sashida_h 2:0deade364b73 142 while(fly == 1); //フライトピン抜けるまで待機
sashida_h 2:0deade364b73 143
sashida_h 15:4ba85a0e8767 144 twe.printf("LAUNCH\r\n");
sashida_h 5:bb9c685fc1fe 145 i = 0;
sashida_h 5:bb9c685fc1fe 146 Alt_buff[0] = 0;
sashida_h 5:bb9c685fc1fe 147 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 5:bb9c685fc1fe 148 if(fp == NULL)twe.printf("ERROR\r\n");
sashida_h 15:4ba85a0e8767 149
sashida_h 2:0deade364b73 150 timer1.start();
sashida_h 15:4ba85a0e8767 151 kaihou.attach(_open,1.0/RATE);
sashida_h 14:0f267807ce7c 152 twe.printf("start!!\r\n");
sashida_h 15:4ba85a0e8767 153
sashida_h 15:4ba85a0e8767 154
sashida_h 15:4ba85a0e8767 155
sashida_h 2:0deade364b73 156 while(1){
sashida_h 2:0deade364b73 157
sashida_h 5:bb9c685fc1fe 158 /* twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para);
sashida_h 5:bb9c685fc1fe 159 wait(1.0);
sashida_h 5:bb9c685fc1fe 160 */
sashida_h 2:0deade364b73 161 /* GPS取得&送信 */
sashida_h 5:bb9c685fc1fe 162 gps_data[cnt_gps] = gps.getc();
sashida_h 2:0deade364b73 163 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 2:0deade364b73 164 cnt_gps = 0;
sashida_h 2:0deade364b73 165 memset(gps_data,'\0',256);
sashida_h 2:0deade364b73 166 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 2:0deade364b73 167 float world_time, lon_east, lat_north;
sashida_h 2:0deade364b73 168 int rlock, sat_num;
sashida_h 2:0deade364b73 169 char lat,lon;
sashida_h 2:0deade364b73 170 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 171 if(rlock==1){
sashida_h 2:0deade364b73 172 lat_north = _DMS2DEG(lat_north);
sashida_h 2:0deade364b73 173 lon_east = _DMS2DEG(lon_east);
sashida_h 2:0deade364b73 174 // pc.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
sashida_h 2:0deade364b73 175 // twe.printf("%s\r\n",gps_data);
sashida_h 2:0deade364b73 176 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 177 }else{
sashida_h 2:0deade364b73 178 // pc.printf("max altitude:%f\r\n",Max_Alt);
sashida_h 2:0deade364b73 179 twe.printf("%s\r\n",gps_data);
sashida_h 2:0deade364b73 180 }
sashida_h 5:bb9c685fc1fe 181 twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt,Cnt_para);
sashida_h 2:0deade364b73 182 }
sashida_h 2:0deade364b73 183 }else{
sashida_h 2:0deade364b73 184 cnt_gps++;
sashida_h 2:0deade364b73 185 }
sashida_h 2:0deade364b73 186
sashida_h 5:bb9c685fc1fe 187 }//while_gps
sashida_h 5:bb9c685fc1fe 188 }//main
sashida_h 5:bb9c685fc1fe 189
sashida_h 5:bb9c685fc1fe 190 void _open(){
sashida_h 5:bb9c685fc1fe 191 bmp.ReadData(&temperature,&pressure);
sashida_h 5:bb9c685fc1fe 192 mpu.getAccelero(a);
sashida_h 5:bb9c685fc1fe 193 mpu.getGyro(d);
sashida_h 5:bb9c685fc1fe 194 Alt_buff2[i] = get_Alt(pressure, temperature);
sashida_h 14:0f267807ce7c 195 c[i] = a[2];
sashida_h 5:bb9c685fc1fe 196 i++;
sashida_h 6:e9004c78b394 197 if(i == 10){ //10回ごとに中央値計算
sashida_h 14:0f267807ce7c 198 if(tf_launch == true){
sashida_h 14:0f267807ce7c 199 Acc_now = median(c, 10);
sashida_h 15:4ba85a0e8767 200 Alt_now = median(Alt_buff2, 10);
sashida_h 15:4ba85a0e8767 201 Alt_now = Alt_now - Alt_gnd;
sashida_h 15:4ba85a0e8767 202 if(Alt_now > Max_Alt) Max_Alt = Alt_now;
sashida_h 15:4ba85a0e8767 203 t = timer1.read();
sashida_h 15:4ba85a0e8767 204 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 14:0f267807ce7c 205 i = 0;
sashida_h 15:4ba85a0e8767 206 if(Acc_now < 1.0 && t > KAIHOU){
sashida_h 15:4ba85a0e8767 207 tf_launch = false;
sashida_h 15:4ba85a0e8767 208 twe.printf("accelero_ok\r\n");
sashida_h 15:4ba85a0e8767 209 }
sashida_h 14:0f267807ce7c 210
sashida_h 14:0f267807ce7c 211 }else{
sashida_h 14:0f267807ce7c 212
sashida_h 14:0f267807ce7c 213 Alt_now = median(Alt_buff2, 10);
sashida_h 14:0f267807ce7c 214 Alt_now = Alt_now - Alt_gnd;
sashida_h 14:0f267807ce7c 215 t = timer1.read();
sashida_h 14:0f267807ce7c 216 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 14:0f267807ce7c 217 if(Alt_now > Max_Alt) Max_Alt = Alt_now;
sashida_h 14:0f267807ce7c 218 i = 0;
sashida_h 14:0f267807ce7c 219 if(tf_para == true){ //パラ開くまでは頂点判定
sashida_h 14:0f267807ce7c 220 Alt_buff[Cnt_buff+1] = Alt_now;
sashida_h 15:4ba85a0e8767 221 if(Alt_buff[Cnt_buff]-Alt_buff[Cnt_buff+1] > ALT) Cnt_para++; //直前の値(0.2秒前よりALTm降下)より小さければカウント+1
sashida_h 14:0f267807ce7c 222 // twe.printf("Cnt_para:%d\r\n", Cnt_para);
sashida_h 14:0f267807ce7c 223 Cnt_buff++;
sashida_h 14:0f267807ce7c 224 if(Cnt_para == 10 || t > TIMER){ //頂点！！！
sashida_h 14:0f267807ce7c 225 kaihou.detach(); //SD閉じる前にサーボ動かす前にTicker止める
sashida_h 14:0f267807ce7c 226 fprintf(fp,"OPEN!\r\n");
sashida_h 14:0f267807ce7c 227 fclose(fp);
sashida_h 14:0f267807ce7c 228 lfp = fopen("/local/Alt.txt","a");
sashida_h 14:0f267807ce7c 229 fprintf(lfp, "MAX:%f\r\n",Max_Alt);
sashida_h 14:0f267807ce7c 230 fclose(lfp);
sashida_h 14:0f267807ce7c 231 _para(UNLOCK);
sashida_h 14:0f267807ce7c 232 tf_para = false;
sashida_h 14:0f267807ce7c 233 timer1.stop();
sashida_h 14:0f267807ce7c 234 twe.printf("PARA_OPEN\r\n");
sashida_h 14:0f267807ce7c 235 twe.printf("GPS_MODE_ON!!!!\r\n");
sashida_h 9:b10aaf72d100 236
sashida_h 14:0f267807ce7c 237 ochiru.attach(_recovery,0.2); //パラメータ保存のためのやつ
sashida_h 5:bb9c685fc1fe 238
sashida_h 14:0f267807ce7c 239 }
sashida_h 5:bb9c685fc1fe 240 }
sashida_h 5:bb9c685fc1fe 241 }
sashida_h 2:0deade364b73 242 }
sashida_h 7:852922a4058a 243
sashida_h 7:852922a4058a 244 if(Cnt_buff == 250) Cnt_buff = 0;
sashida_h 5:bb9c685fc1fe 245
mbed_official 0:bdbd3d6fc5d5 246 }
sashida_h 2:0deade364b73 247
sashida_h 5:bb9c685fc1fe 248 void _recovery(){
sashida_h 2:0deade364b73 249
sashida_h 7:852922a4058a 250 if(i == 0){
sashida_h 2:0deade364b73 251 fp = fopen("/sd/mydir/sdtest.txt", "a");
sashida_h 2:0deade364b73 252 if(fp == NULL)twe.printf("ERROR\r\n");
sashida_h 2:0deade364b73 253 }
sashida_h 2:0deade364b73 254
sashida_h 5:bb9c685fc1fe 255 bmp.ReadData(&temperature,&pressure);
sashida_h 5:bb9c685fc1fe 256 mpu.getAccelero(a);
sashida_h 5:bb9c685fc1fe 257 mpu.getGyro(d);
sashida_h 5:bb9c685fc1fe 258 Alt_now = get_Alt(pressure, temperature);
sashida_h 5:bb9c685fc1fe 259 Alt_now = Alt_now - Alt_gnd;
sashida_h 5:bb9c685fc1fe 260 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 9:b10aaf72d100 261 i++;
sashida_h 2:0deade364b73 262
sashida_h 7:852922a4058a 263 if(i == 50){
sashida_h 5:bb9c685fc1fe 264 fclose(fp);
sashida_h 9:b10aaf72d100 265 i = 0;
sashida_h 2:0deade364b73 266 }
sashida_h 2:0deade364b73 267 }
sashida_h 2:0deade364b73 268 float get_Alt(float press, float temp){
sashida_h 2:0deade364b73 269 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
sashida_h 2:0deade364b73 270 }
sashida_h 2:0deade364b73 271
sashida_h 2:0deade364b73 272 float median(float data[], int num){//todo:処理時間計測
sashida_h 2:0deade364b73 273 float *data_cpy, ans;
sashida_h 2:0deade364b73 274 data_cpy = new float[num];
sashida_h 2:0deade364b73 275 memcpy(data_cpy,data,sizeof(float)*num);
sashida_h 2:0deade364b73 276
sashida_h 2:0deade364b73 277 for(int i=0; i<num; i++){
sashida_h 2:0deade364b73 278 for(int j=0; j<num-i-1; j++){
sashida_h 2:0deade364b73 279 if(data_cpy[j]>data_cpy[j+1]){
sashida_h 2:0deade364b73 280 float buff = data_cpy[j+1];
sashida_h 2:0deade364b73 281 data_cpy[j+1] = data_cpy[j];
sashida_h 2:0deade364b73 282 data_cpy[j] = buff;
sashida_h 2:0deade364b73 283 }
sashida_h 2:0deade364b73 284 }
sashida_h 2:0deade364b73 285 }
sashida_h 2:0deade364b73 286
sashida_h 2:0deade364b73 287 if(num%2!=0) ans = data_cpy[num/2];
sashida_h 2:0deade364b73 288 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
sashida_h 2:0deade364b73 289 delete[] data_cpy;
sashida_h 2:0deade364b73 290 return ans;
sashida_h 2:0deade364b73 291 }
sashida_h 2:0deade364b73 292
sashida_h 2:0deade364b73 293
sashida_h 2:0deade364b73 294 float _DMS2DEG(float raw_data){
sashida_h 2:0deade364b73 295 int d = (int)(raw_data/100);
sashida_h 2:0deade364b73 296 float m = (raw_data - (float)d*100);
sashida_h 2:0deade364b73 297 return (float)d + m/60;
sashida_h 2:0deade364b73 298 }
sashida_h 2:0deade364b73 299
sashida_h 2:0deade364b73 300 void _para(int motion){
sashida_h 2:0deade364b73 301 if(motion==UNLOCK){
sashida_h 15:4ba85a0e8767 302 servo_para.pulsewidth(0.0008); // pulse servo out sita
sashida_h 15:4ba85a0e8767 303 servo_para2.pulsewidth(0.0008);
sashida_h 2:0deade364b73 304 }else if(motion==LOCK){
sashida_h 15:4ba85a0e8767 305 servo_para.pulsewidth(0.0023); // pulse servo outu sita
sashida_h 15:4ba85a0e8767 306 servo_para2.pulsewidth(0.0023);
sashida_h 2:0deade364b73 307 }
sashida_h 10:273500c77873 308 }
sashida_h 10:273500c77873 309
sashida_h 10:273500c77873 310 int _input(char c){
sashida_h 10:273500c77873 311
sashida_h 10:273500c77873 312 if(c=='f'){
sashida_h 10:273500c77873 313 return 3;
sashida_h 10:273500c77873 314 }else if(c=='l'){
sashida_h 10:273500c77873 315 return 2;
sashida_h 10:273500c77873 316 }else if(c=='u'){
sashida_h 10:273500c77873 317 return 1;
sashida_h 10:273500c77873 318 }
sashida_h 10:273500c77873 319 return 0;
sashida_h 2:0deade364b73 320 }