CORE / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@12:6a3c0e9075eb, 2019-03-10 (annotated)

Committer:
sashida_h
Date:
Sun Mar 10 11:47:23 2019 +0000
Revision:
12:6a3c0e9075eb
Parent:
11:c90db1500720
Child:
13:3e6411bfd581
2019_0310_2047

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:0ff20d8e9090 1 #include "mbed.h"
Yukina 4:4b3ae90ec778 2 #include "MPU6050.h"
mikawataru 0:0ff20d8e9090 3 #include "BMP180.h"
mikawataru 0:0ff20d8e9090 4
Yukina 5:f6e956e8a060 5 /*しきい値など*/
sashida_h 11:c90db1500720 6 #define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値
sashida_h 8:15a1b22df82f 7 #define TIME_BURNING 6 //開放判定しない時間（燃焼時間）
sashida_h 8:15a1b22df82f 8 #define ALT_JUDGE_OPEN 1 //落下判定のカウントを１増やす高度差
sashida_h 8:15a1b22df82f 9 #define TIME_OPEN 25 //強制的に開放させる時間
sashida_h 11:c90db1500720 10 #define TIME_SEND 1.0 //無線送信する間隔
sashida_h 11:c90db1500720 11 #define CNT_JUDGE 10 //頂点判定する時に落下のカウント数
sashida_h 11:c90db1500720 12 #define NUM_CNT_MEDIAN 10 //中央値をとる個数
sashida_h 11:c90db1500720 13 #define RATE_GPS 1.0 //GPSのTickerを動作させるタイミング
sashida_h 11:c90db1500720 14 #define p0 1013.25f //海面気圧
Yukina 5:f6e956e8a060 15
sashida_h 7:9953d922499d 16 MPU6050 mpu(PB_7,PB_6);
sashida_h 7:9953d922499d 17 BMP180 bmp(PB_7,PB_6);
sashida_h 7:9953d922499d 18 Serial pc(PA_2, PA_3);
sashida_h 7:9953d922499d 19 Serial gps(PA_9, PA_10);
sashida_h 9:42b4d337d4cc 20 DigitalOut myled(PA_15);
sashida_h 10:1a626929850e 21 DigitalOut ES920_RST(PA_5);
sashida_h 11:c90db1500720 22 PwmOut servo1(PB_4);
sashida_h 11:c90db1500720 23 PwmOut servo2(PB_5);
sashida_h 11:c90db1500720 24 DigitalOut servo1_signal(PA_0);
sashida_h 11:c90db1500720 25 DigitalOut servo2_signal(PA_1);
Yukina 5:f6e956e8a060 26 Timer timer_open;
Yukina 5:f6e956e8a060 27 Timer timer_data;
Yukina 5:f6e956e8a060 28 Ticker tic_gps;
mikawataru 0:0ff20d8e9090 29
Yukina 5:f6e956e8a060 30 /*自作関数*/
Yukina 5:f6e956e8a060 31 float _getAlt();
Yukina 5:f6e956e8a060 32 float _median(float data[],int num);
Yukina 5:f6e956e8a060 33 void _SendGPS();
Yukina 5:f6e956e8a060 34 float _DMS2DEG(float raw_data);
Yukina 4:4b3ae90ec778 35
Yukina 5:f6e956e8a060 36 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
Yukina 4:4b3ae90ec778 37
Yukina 5:f6e956e8a060 38 /*グローバル変数*/
sashida_h 7:9953d922499d 39 //地上高度
sashida_h 7:9953d922499d 40 float alt_gnd;
sashida_h 9:42b4d337d4cc 41 float alt_max;
sashida_h 11:c90db1500720 42 char c[3];
sashida_h 9:42b4d337d4cc 43 int i = 0;
sashida_h 9:42b4d337d4cc 44 void main(){
sashida_h 10:1a626929850e 45 ES920_RST = 0;
sashida_h 10:1a626929850e 46 myled =0;
Yukina 5:f6e956e8a060 47 /*ローカル変数*/
Yukina 5:f6e956e8a060 48 float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
sashida_h 9:42b4d337d4cc 49 float alt_buff[10],alt_md;
Yukina 5:f6e956e8a060 50 float time_judge;
Yukina 5:f6e956e8a060 51 int cnt_data=0,cnt_judge=0;
sashida_h 11:c90db1500720 52 float t = 0;
Yukina 5:f6e956e8a060 53 /*センサの初期化等*/
sashida_h 7:9953d922499d 54 pc.baud(38400);
Yukina 5:f6e956e8a060 55 mpu.setAcceleroRange(3);
mikawataru 0:0ff20d8e9090 56 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
Yukina 5:f6e956e8a060 57
Yukina 5:f6e956e8a060 58 /*初期位置の設定*/
Yukina 5:f6e956e8a060 59 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 60 mpu.getGyro(gyro);
Yukina 5:f6e956e8a060 61
Yukina 5:f6e956e8a060 62 Phase = STANDBY;
sashida_h 7:9953d922499d 63 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
sashida_h 7:9953d922499d 64 alt_buff[cnt_data] = _getAlt();
sashida_h 7:9953d922499d 65 }
sashida_h 7:9953d922499d 66 alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN);
sashida_h 7:9953d922499d 67 wait(2.0);
sashida_h 12:6a3c0e9075eb 68 pc.printf("0,0,0,0,0\r\n");
sashida_h 8:15a1b22df82f 69 wait(1.0);
sashida_h 9:42b4d337d4cc 70 timer_data.start();
sashida_h 11:c90db1500720 71 servo1_signal = 1;
sashida_h 11:c90db1500720 72 servo2_signal = 1;
sashida_h 11:c90db1500720 73 servo1.pulsewidth(0.0024);
sashida_h 11:c90db1500720 74 servo1.pulsewidth(0.0024);
Yukina 5:f6e956e8a060 75 while(1){
Yukina 5:f6e956e8a060 76 switch(Phase){
Yukina 5:f6e956e8a060 77 case STANDBY:
sashida_h 10:1a626929850e 78 /*サーボ入力待ち*/
sashida_h 10:1a626929850e 79 //servo();
sashida_h 10:1a626929850e 80 c[0]=pc.getc();
sashida_h 11:c90db1500720 81 if(c[0] == 'o'){
sashida_h 11:c90db1500720 82 myled = 1;
sashida_h 11:c90db1500720 83 servo1.pulsewidth(0.0006);
sashida_h 11:c90db1500720 84 servo2.pulsewidth(0.0006);
sashida_h 11:c90db1500720 85 mpu.getAccelero(acc);
sashida_h 12:6a3c0e9075eb 86 //pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
sashida_h 12:6a3c0e9075eb 87 pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),timer_data.read());
sashida_h 11:c90db1500720 88 }
sashida_h 11:c90db1500720 89 if(c[0] == 'l'){
sashida_h 11:c90db1500720 90 myled = 0;
sashida_h 11:c90db1500720 91 servo1.pulsewidth(0.0024);
sashida_h 11:c90db1500720 92 servo2.pulsewidth(0.0024);
sashida_h 11:c90db1500720 93 mpu.getAccelero(acc);
sashida_h 12:6a3c0e9075eb 94 //pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
sashida_h 12:6a3c0e9075eb 95 pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),timer_data.read());
sashida_h 11:c90db1500720 96 }
sashida_h 10:1a626929850e 97 if(c[0] == 'f'){
sashida_h 10:1a626929850e 98 //pc.printf("flight_mode\r\n");
sashida_h 10:1a626929850e 99 Phase = LAUNCH;
sashida_h 11:c90db1500720 100 servo1_signal = 0;
sashida_h 11:c90db1500720 101 servo2_signal = 0;
sashida_h 10:1a626929850e 102 }
Yukina 5:f6e956e8a060 103 break;
sashida_h 8:15a1b22df82f 104
Yukina 5:f6e956e8a060 105 case LAUNCH:
Yukina 5:f6e956e8a060 106 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 107 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 108 acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
Yukina 5:f6e956e8a060 109 }
Yukina 5:f6e956e8a060 110 acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
sashida_h 8:15a1b22df82f 111 if(timer_data.read() - t > TIME_SEND){
sashida_h 12:6a3c0e9075eb 112 //pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge);
sashida_h 12:6a3c0e9075eb 113 pc.printf("1,%f,%f,0,%3f\r\n",acc_abs,acc_abs,timer_data.read());
sashida_h 12:6a3c0e9075eb 114
sashida_h 8:15a1b22df82f 115 t = timer_data.read();
sashida_h 7:9953d922499d 116 }
Yukina 5:f6e956e8a060 117 /*加速度判定*/
Yukina 5:f6e956e8a060 118 if(acc_abs>ACC_JUDGE_LAUNCH){
Yukina 5:f6e956e8a060 119 cnt_judge++;
Yukina 4:4b3ae90ec778 120 }
sashida_h 8:15a1b22df82f 121 if(cnt_judge==CNT_JUDGE){
sashida_h 11:c90db1500720 122 servo1_signal = 1;
sashida_h 11:c90db1500720 123 servo1_signal = 1;
Yukina 5:f6e956e8a060 124 cnt_judge=0;
Yukina 5:f6e956e8a060 125 timer_open.start();
sashida_h 7:9953d922499d 126 Phase = RISE;
Yukina 5:f6e956e8a060 127 }
Yukina 5:f6e956e8a060 128 break;
sashida_h 8:15a1b22df82f 129
Yukina 5:f6e956e8a060 130 case RISE:
Yukina 5:f6e956e8a060 131 while(timer_open.read() < TIME_BURNING){
sashida_h 12:6a3c0e9075eb 132 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
sashida_h 12:6a3c0e9075eb 133 alt_buff[cnt_data] = _getAlt();
sashida_h 12:6a3c0e9075eb 134 }
sashida_h 12:6a3c0e9075eb 135 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
sashida_h 12:6a3c0e9075eb 136 alt_md = alt_md - alt_gnd;
sashida_h 12:6a3c0e9075eb 137 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
sashida_h 12:6a3c0e9075eb 138 mpu.getAccelero(acc);
sashida_h 12:6a3c0e9075eb 139 acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
sashida_h 12:6a3c0e9075eb 140 }
sashida_h 12:6a3c0e9075eb 141 acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
sashida_h 12:6a3c0e9075eb 142 //pc.printf("RISE,time from launch:%f\r\n",timer_open.read());
sashida_h 12:6a3c0e9075eb 143 pc.printf("3,%f,%f,%f,%3f\r\n",acc_abs,acc_abs,alt_md,timer_open.read());
sashida_h 12:6a3c0e9075eb 144 wait(0.9);
sashida_h 7:9953d922499d 145 i=0;
sashida_h 8:15a1b22df82f 146 timer_data.reset();
Yukina 5:f6e956e8a060 147 }
sashida_h 8:15a1b22df82f 148 Phase = OPEN;
sashida_h 8:15a1b22df82f 149 t = 0.0;
Yukina 5:f6e956e8a060 150 break;
Yukina 5:f6e956e8a060 151 case FIRE:
Yukina 5:f6e956e8a060 152 break;
Yukina 5:f6e956e8a060 153 case OPEN:
Yukina 5:f6e956e8a060 154 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 155 alt_buff[cnt_data] = _getAlt();
Yukina 5:f6e956e8a060 156 }
Yukina 5:f6e956e8a060 157 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
sashida_h 7:9953d922499d 158 alt_md = alt_md - alt_gnd;
sashida_h 8:15a1b22df82f 159 if(timer_open.read() - t > TIME_SEND){
sashida_h 12:6a3c0e9075eb 160 //pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge);
sashida_h 12:6a3c0e9075eb 161 pc.printf("15,%f,%f,%f,%3f\r\n",acc_abs,acc_abs,alt_md,timer_open.read());
sashida_h 8:15a1b22df82f 162 t = timer_open.read();
sashida_h 7:9953d922499d 163 }
Yukina 5:f6e956e8a060 164 if(alt_md > alt_max){
Yukina 5:f6e956e8a060 165 alt_max = alt_md;
Yukina 5:f6e956e8a060 166 cnt_judge = 0;
Yukina 5:f6e956e8a060 167 }
Yukina 5:f6e956e8a060 168 else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
Yukina 5:f6e956e8a060 169 cnt_judge++;
Yukina 5:f6e956e8a060 170 }
Yukina 4:4b3ae90ec778 171
sashida_h 7:9953d922499d 172 //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
sashida_h 8:15a1b22df82f 173 if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){
Yukina 5:f6e956e8a060 174 Phase = RECOVERY;
sashida_h 12:6a3c0e9075eb 175 pc.printf("9,0,0,0,0\r\n");
sashida_h 12:6a3c0e9075eb 176 wait(0.5);
sashida_h 9:42b4d337d4cc 177 tic_gps.attach(&_SendGPS, 1.0/RATE_GPS);
Yukina 5:f6e956e8a060 178 }
Yukina 5:f6e956e8a060 179 break;
Yukina 5:f6e956e8a060 180 case RECOVERY:
Yukina 5:f6e956e8a060 181 break;
Yukina 5:f6e956e8a060 182 case SEA:
Yukina 5:f6e956e8a060 183 break;
Yukina 4:4b3ae90ec778 184 }
Yukina 4:4b3ae90ec778 185 }
Yukina 4:4b3ae90ec778 186 }
Yukina 4:4b3ae90ec778 187
Yukina 4:4b3ae90ec778 188 float _getAlt(){
Yukina 4:4b3ae90ec778 189 float altitude,pressure,temperature;
Yukina 4:4b3ae90ec778 190 bmp.ReadData(&temperature,&pressure);
Yukina 4:4b3ae90ec778 191 altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
Yukina 4:4b3ae90ec778 192 return altitude;
Yukina 4:4b3ae90ec778 193 }
Yukina 4:4b3ae90ec778 194
sashida_h 9:42b4d337d4cc 195 float _DMS2DEG(float raw_data){
sashida_h 9:42b4d337d4cc 196 int d=(int)(raw_data/100);
sashida_h 9:42b4d337d4cc 197 float m=(raw_data-(float)d*100);
sashida_h 9:42b4d337d4cc 198 return (float)d+m/60;
sashida_h 9:42b4d337d4cc 199 }
sashida_h 9:42b4d337d4cc 200
sashida_h 9:42b4d337d4cc 201
sashida_h 9:42b4d337d4cc 202 float _median(float data[], int num){
sashida_h 9:42b4d337d4cc 203 float *data_cpy, ans;
sashida_h 9:42b4d337d4cc 204 data_cpy = new float[num];
sashida_h 9:42b4d337d4cc 205 memcpy(data_cpy,data,sizeof(float)*num);
sashida_h 9:42b4d337d4cc 206
sashida_h 9:42b4d337d4cc 207 for(i=0; i<num; i++){
sashida_h 9:42b4d337d4cc 208 for(int j=0; j<num-i-1; j++){
sashida_h 9:42b4d337d4cc 209 if(data_cpy[j]>data_cpy[j+1]){
sashida_h 9:42b4d337d4cc 210 float buff = data_cpy[j+1];
sashida_h 9:42b4d337d4cc 211 data_cpy[j+1] = data_cpy[j];
sashida_h 9:42b4d337d4cc 212 data_cpy[j] = buff;
sashida_h 9:42b4d337d4cc 213 }
sashida_h 9:42b4d337d4cc 214 }
sashida_h 9:42b4d337d4cc 215 }
sashida_h 9:42b4d337d4cc 216
sashida_h 9:42b4d337d4cc 217 if(num%2!=0) ans = data_cpy[num/2];
sashida_h 9:42b4d337d4cc 218 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
sashida_h 9:42b4d337d4cc 219 delete[] data_cpy;
sashida_h 9:42b4d337d4cc 220 return ans;
sashida_h 9:42b4d337d4cc 221 }
sashida_h 9:42b4d337d4cc 222
Yukina 5:f6e956e8a060 223 void _SendGPS(){
sashida_h 7:9953d922499d 224 char gps_data[256];
sashida_h 11:c90db1500720 225 int cnt_gps=0;
sashida_h 7:9953d922499d 226 while(1){
sashida_h 7:9953d922499d 227 if(gps.readable()){
sashida_h 7:9953d922499d 228 gps_data[cnt_gps] = gps.getc();
sashida_h 7:9953d922499d 229 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 7:9953d922499d 230 cnt_gps = 0;
sashida_h 7:9953d922499d 231 memset(gps_data,'\0',256);
sashida_h 7:9953d922499d 232 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 7:9953d922499d 233 float world_time, lon_east, lat_north;
sashida_h 7:9953d922499d 234 int rlock, sat_num;
sashida_h 7:9953d922499d 235 char lat,lon;
sashida_h 7:9953d922499d 236 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 7:9953d922499d 237 if(rlock==1){
sashida_h 7:9953d922499d 238 lat_north = _DMS2DEG(lat_north);
sashida_h 7:9953d922499d 239 lon_east = _DMS2DEG(lon_east);
sashida_h 7:9953d922499d 240 //pc.printf("%s\r\n",gps_data);
sashida_h 7:9953d922499d 241 //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
sashida_h 7:9953d922499d 242 int japan_time = int(world_time) - 9;
sashida_h 12:6a3c0e9075eb 243 pc.printf("Lat,%f,Lon,%f,MAX_ALT,%f\r\n",lat_north,lon_east,alt_max);
sashida_h 10:1a626929850e 244 for(i=0;i<2;i++){
sashida_h 10:1a626929850e 245 c[i]=pc.getc();
sashida_h 10:1a626929850e 246 }
sashida_h 10:1a626929850e 247 //pc.printf("%c",c[1]);
sashida_h 10:1a626929850e 248 if(c[1]!='O'){
sashida_h 10:1a626929850e 249 ES920_RST = 1;
sashida_h 10:1a626929850e 250 wait(0.1);
sashida_h 10:1a626929850e 251 ES920_RST = 0;
sashida_h 10:1a626929850e 252 wait(1.0);
sashida_h 10:1a626929850e 253 myled = 1;
sashida_h 10:1a626929850e 254 }else{
sashida_h 10:1a626929850e 255 myled = 0;
sashida_h 10:1a626929850e 256 }
sashida_h 10:1a626929850e 257
sashida_h 7:9953d922499d 258 break;
sashida_h 7:9953d922499d 259 }else{
sashida_h 7:9953d922499d 260 //pc.printf("%s\r\n",gps_data);
sashida_h 7:9953d922499d 261 pc.printf("NoGPSSignal\r\n");
sashida_h 7:9953d922499d 262 break;
sashida_h 7:9953d922499d 263 }
Yukina 5:f6e956e8a060 264 }else{
sashida_h 7:9953d922499d 265 //ffpc.printf("No_Satellite_signal\r\n");
Yukina 5:f6e956e8a060 266 }
sashida_h 7:9953d922499d 267 }else{
sashida_h 7:9953d922499d 268 cnt_gps++;
Yukina 5:f6e956e8a060 269 }
Yukina 5:f6e956e8a060 270 }
Yukina 4:4b3ae90ec778 271 }
Yukina 4:4b3ae90ec778 272
sashida_h 7:9953d922499d 273 }