Haruki Sashida
201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 7:9953d922499d
- Parent:
- 6:f17310205c1f
- Child:
- 8:15a1b22df82f
diff -r f17310205c1f -r 9953d922499d main.cpp
--- a/main.cpp Tue Feb 12 11:20:33 2019 +0000
+++ b/main.cpp Mon Mar 04 02:50:16 2019 +0000
@@ -5,24 +5,27 @@
#include "MadgwickAHRS.h"
/*しきい値など*/
-#define ACC_JUDGE_LAUNCH 1.5
+#define ACC_JUDGE_LAUNCH 3.0
#define TIME_BURNING 6
-#define ALT_JUDGE_FIRE 1
-#define ALT_JUDGE_OPEN 2
-#define ANGLE_JUDGE_FIRE 90
-#define CNT_JUDGE 3
+#define ALT_JUDGE_FIRE 0
+#define ALT_JUDGE_OPEN 1
+#define ANGLE_JUDGE_FIRE_MIN 15
+#define ANGLE_JUDGE_FIRE_MAX 70
+#define CNT_JUDGE 10
#define TIME_JUDGE_CNT 1.5
#define NUM_CNT_MEDIAN 10
-#define RATE_GPS 1
-#define RATE_DATA 2
+#define RATE_GPS 1.0
+#define RATE_DATA 10
+#define TIMER_NOTFIRE 15.0
#define p0 1013.25f
#define RadToDeg 57.295779513082320876798154814105
-MPU6050 mpu(dp5,dp27);
-BMP180 bmp(dp5,dp27);
+MPU6050 mpu(PB_7,PB_6);
+BMP180 bmp(PB_7,PB_6);
KalmanFilter gKfx, gKfy;
Madgwick MadgwickFilter;
-Serial pc(dp16,dp15);
+Serial pc(PA_2, PA_3);
+Serial gps(PA_9, PA_10);
Timer timer_open;
Timer timer_data;
Ticker tic_data;
@@ -43,16 +46,23 @@
float gCalibrateY;
float gPrevMicros;
float degRoll,degPitch;
+//地上高度
+float alt_gnd;
+//GPS
+int cnt_gps=0;
+
+int p = 1;
int main(){
/*ローカル変数*/
float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
- float alt_buff[10],alt_md,alt_max;
+ float alt_buff[10],alt_md,alt_max=0;
float time_judge;
int cnt_data=0,cnt_judge=0;
+ int i=0;
/*センサの初期化等*/
- pc.baud(9600);
+ pc.baud(38400);
mpu.setAcceleroRange(3);
bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
@@ -67,14 +77,44 @@
gCalibrateX = degRoll;
Phase = STANDBY;
+ for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
+ alt_buff[cnt_data] = _getAlt();
+ }
+ alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN);
+ wait(2.0);
+ pc.printf("Hello World!\r\n");
+ timer_data.start();
+ int r = 0;
+ float r_time;
+ pc.printf("TimerStart,Standby_mode\r\n");
while(1){
switch(Phase){
case STANDBY:
/*入力待ち*/
timer_data.start();
- tic_data.attach(&_SendData, 1.0/RATE_DATA);
- Phase = LAUNCH;
+ //tic_data.attach(&_SendData, 1.0/RATE_DATA);
+ if(r == 0) r_time = timer_data.read() / 60.0;
+ if(r == 1) r_time = (timer_data.read() + 1800.0)/60;
+ if(r == 2) r_time = (timer_data.read() + 3600.0)/60;
+ if(r == 3) r_time = (timer_data.read() + 5400.0)/60;
+ if(r == 4){
+ timer_data.reset();
+ Phase = LAUNCH;
+ pc.printf("Flight_mode\r\n");
+ }
+ pc.printf("TIME:%3f[min]",r_time);
+ if(timer_data.read() > 1800){
+ timer_data.reset();
+ r++;
+ }
+ /*char c = pc.getc();
+ if(c=='f'){
+ pc.printf("Flight_mode\r\n");
+ Phase = LAUNCH;
+ }
+ */
+ wait(1.0);
break;
case LAUNCH:
for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
@@ -82,25 +122,56 @@
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));
}
acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
+ if(i==50){
+ pc.printf("LAUNCH,acc:%f,time:%f\r\n",acc_abs,timer_data.read());
+ i=0;
+ }
+ i++;
/*加速度判定*/
if(acc_abs>ACC_JUDGE_LAUNCH){
cnt_judge++;
}
- if(cnt_judge==CNT_JUDGE){
+ if(cnt_judge==CNT_JUDGE || timer_data.read()>30.0){
cnt_judge=0;
timer_open.start();
- Phase = FIRE;
+ Phase = RISE;
+ pc.printf("LAUNCH!!!!\r\n");
}
break;
case RISE:
while(timer_open.read() < TIME_BURNING){
+ pc.printf("RISE,time:%f\r\n",timer_open.read());
+ wait(0.5);
+ i=0;
+ }
+ if(p == 1){
+ pc.printf("FIRE_JUDGE_START\r\n");
+ p=0;
}
for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
alt_buff[cnt_data] = _getAlt();
}
alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
+ alt_md = alt_md - alt_gnd;
if(alt_md > ALT_JUDGE_FIRE){
Phase = FIRE;
+ if(i == 5){
+ //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md);
+ pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md);
+ i = 0;
+ }
+ }else{
+ if(i == 5){
+ //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md);
+ pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md);
+ i = 0;
+ }
+ }
+ i++;
+ //if(timer_open.read()>TIMER_NOTFIRE){
+ if(timer_open.read()>60.0){
+ Phase = OPEN;
+ //pc.printf("NOT_FIRE!\r\n");
}
break;
case FIRE:
@@ -143,16 +214,35 @@
Roll = MadgwickFilter.getRoll();
Pitch = MadgwickFilter.getPitch();
Yaw = MadgwickFilter.getYaw();
- if(Pitch>ANGLE_JUDGE_FIRE){
+ i++;
+ if(i==400){
+ //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch);
+ pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read());
+ i=0;
+ }
+ /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){
Phase = OPEN;
+ i=0;
}
+ */
+ //if(timer_open.read()>TIMER_NOTFIRE){
+ if(timer_data.read()>60.0){
+ Phase = OPEN;
+ pc.printf("NOT_FIRE!!\r\n");
+ }
break;
case OPEN:
for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
alt_buff[cnt_data] = _getAlt();
}
alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
+ alt_md = alt_md - alt_gnd;
+ if(i==10){
+ pc.printf("OPEN,Cnt:%d,ALT_NOW:%f,ALT_MAX%f\r\n",cnt_judge,alt_md,alt_max);
+ i=0;
+ }
+ i++;
if(alt_md > alt_max){
alt_max = alt_md;
cnt_judge = 0;
@@ -162,18 +252,30 @@
time_judge = timer_open.read();
}
- if((timer_open.read()-time_judge) - TIME_JUDGE_CNT) cnt_judge=0;
- if(cnt_judge == CNT_JUDGE){
+ //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
+ //if(cnt_judge == CNT_JUDGE){
+ if(timer_data.read() > 70.0){
cnt_judge = 0;
Phase = RECOVERY;
+ pc.printf("Fall!!!\r\n");
+ wait(1.0);
+
}
break;
case RECOVERY:
- tic_data.detach();
+ //tic_data.detach();
//tic_gps.attach(&_SendGPS, 1.0/RATE_GPS);
Phase = SEA;
break;
case SEA:
+ char c;
+ while(1){
+ if(gps.readable()){
+ c = gps.getc();
+ pc.printf("%c",c);
+ }
+ }
+
break;
}
}
@@ -224,37 +326,47 @@
Yaw = MadgwickFilter.getYaw();
alt = _getAlt();
-
+ alt = alt - alt_gnd;
//pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy);
pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
}
-/*
+
void _SendGPS(){
- int cnt_gps;
- if(gps.readable()){
- gps_data[cnt_gps] = gps.getc();
- if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
- cnt_gps = 0;
- memset(gps_data,'\0',256);
- }else if(gps_data[cnt_gps] == '\r'){
- float world_time, lon_east, lat_north;
- int rlock, sat_num;
- char lat,lon;
- if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
- if(rlock==1){
- lat_north = _DMS2DEG(lat_north);
- lon_east = _DMS2DEG(lon_east);
- twe.printf("%s\r\n",gps_data);
- twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+ char gps_data[256];
+ while(1){
+ if(gps.readable()){
+ gps_data[cnt_gps] = gps.getc();
+ if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
+ cnt_gps = 0;
+ memset(gps_data,'\0',256);
+ }else if(gps_data[cnt_gps] == '\r'){
+ float world_time, lon_east, lat_north;
+ int rlock, sat_num;
+ char lat,lon;
+ if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
+ if(rlock==1){
+ lat_north = _DMS2DEG(lat_north);
+ lon_east = _DMS2DEG(lon_east);
+ //pc.printf("%s\r\n",gps_data);
+ //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+ int japan_time = int(world_time) - 9;
+ pc.printf("Lat:%f,Lon:%f,ntime:%d\r\n",lat_north,lon_east,world_time);
+ break;
+ }else{
+ //pc.printf("%s\r\n",gps_data);
+ pc.printf("NoGPSSignal\r\n");
+ break;
+ }
}else{
- twe.printf("%s\r\n",gps_data);
+ //ffpc.printf("No_Satellite_signal\r\n");
}
+ }else{
+ cnt_gps++;
}
- }else{
- cnt_gps++;
}
}
+
}
float _DMS2DEG(float raw_data){
@@ -262,7 +374,7 @@
float m=(raw_data-(float)d*100);
return (float)d+m/60;
}
-*/
+
float _median(float data[], int num){
float *data_cpy, ans;
@@ -283,4 +395,4 @@
else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
delete[] data_cpy;
return ans;
-}
\ No newline at end of file
+}
\ No newline at end of file