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@3:a7b39e55d100, 2018-02-20 (annotated)

Committer:
sashida_h
Date:
Tue Feb 20 03:14:24 2018 +0000
Revision:
3:a7b39e55d100
Parent:
2:0deade364b73
Child:
4:44e13dd5250b
??????;

Who changed what in which revision?

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