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

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

Committer:
sashida_h
Date:
Sun Jun 23 09:52:57 2019 +0000
Revision:
20:89fdec3731f8
Parent:
19:1e1a9f1829a9
add description;

Who changed what in which revision?

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