Haruki Sashida
201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 5:f6e956e8a060
- Parent:
- 4:4b3ae90ec778
- Child:
- 6:f17310205c1f
--- a/main.cpp Thu Aug 09 21:39:18 2018 +0000
+++ b/main.cpp Sun Dec 30 13:28:35 2018 +0000
@@ -1,273 +1,159 @@
#include "mbed.h"
-#include "math.h"
#include "MPU6050.h"
#include "BMP180.h"
-#include "SDFileSystem.h"
-
-/*マクロ処理*/
-//動作レート
-#define RATE_LOG 20
-#define RATE_OPEN 50
-//発射判定
-#define ACC_JUDGE_LAUNCH 1.5
-#define CNT_JUDGE_LAUNCH 5
-//待機時間
-#define TIME_GAP 3.0
-//頂点判定
-#define ALT_JUDGE_OPEN 1.5
-#define CNT_JUDGE_OPEN 5
-#define TIME_JUDGE_OPEN 30
-//放出判定
-#define TIME_JUDGE_OUT 60
-#define ALT_JUDGE_OUT -1.5
-#define CNT_JUDGE_OUT 5
-//共通
-#define TIME_JUDGE_CNT 1.5
-#define p0 1013.25f
-//ログ //2018.06.28 edit
-#define NUM_DATA 20
+#include "Kalman.h"
+#include "MadgwickAHRS.h"
-MPU6050 mpu(p9,p10);
-BMP180 bmp(p9,p10);
-BusOut led(LED1,LED2,LED3,LED4);
-Serial pc(USBTX,USBRX);
-Serial twe(p28,p27);
-Serial gps(p13,p14);
-PwmOut servo1(p21);
-PwmOut servo2(p22);
-PwmOut servo3(p23);
-PwmOut servo4(p24);
-DigitalOut sw(p25);
-SDFileSystem sd(p5,p6,p7,p8,"sd");
-LocalFileSystem local("local");
-
-Timer timer_open;
-Timer timer_log;
-Ticker tic_open;
-Ticker tic_log;
+/*しきい値など*/
+#define ACC_JUDGE_LAUNCH 1.5
+#define TIME_BURNING 6
+#define ALT_JUDGE_FIRE 1
+#define ALT_JUDGE_OPEN 2
+#define ANGLE_JUDGE_FIRE 90
+#define CNT_JUDGE 3
+#define TIME_JUDGE_CNT 1.5
+#define NUM_CNT_MEDIAN 10
+#define RATE_GPS 1
+#define RATE_DATA 2
+#define p0 1013.25f
+#define RadToDeg 57.295779513082320876798154814105
+
+MPU6050 mpu(dp5,dp27);
+BMP180 bmp(dp5,dp27);
+KalmanFilter gKfx, gKfy;
+Madgwick MadgwickFilter;
+Serial pc(dp16,dp15);
+Timer timer_open;
+Timer timer_data;
+Ticker tic_data;
+Ticker tic_gps;
-void _open();
-void _log();
-float _getAlt();
-float _DMS2DEG(float raw_data);
-float _median(float data[],int num);
-int _input(char cha);
+/*自作関数*/
+float _getAlt();
+float _median(float data[],int num);
+void _SendData();
+void _SendGPS();
+float _DMS2DEG(float raw_data);
-float Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること
-float Time_alt;
-float Data[2][NUM_DATA][8];
-int Cnt=0, Cnt_acc=0, Cnt_alt=0;
-int Cnt_data=0;
-int cnt_gps;
-char gps_data[256];
-bool Row;
-FILE *lfp;
-FILE *fp;
+enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
-enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase;
-enum SERVO{LOCK=0,UNLOCK=1} Servo;
+/*グローバル変数*/
+//カルマン用
+float gCalibrateX;
+float gCalibrateY;
+float gPrevMicros;
+float degRoll,degPitch;
int main(){
- /*センサ類初期化*/
+ /*ローカル変数*/
+ float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
+ float alt_buff[10],alt_md,alt_max;
+ float time_judge;
+ int cnt_data=0,cnt_judge=0;
+
+ /*センサの初期化等*/
+ pc.baud(9600);
+ mpu.setAcceleroRange(3);
bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
- twe.baud(115200);
- sw=1;
- /*ファイル作成*/
- mkdir("/sd/mydir",0777);
- fp=fopen("/sd/mydir/data.txt","a");
- fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
- fclose(fp);
- lfp=fopen("/local/data.txt","a");
- fclose(lfp);
- /*サーボ調整*/
- servo1.period_ms(20);
- servo2.period_ms(20);
- servo3.period_ms(20);
- servo4.period_ms(20);
- /*初めの挨拶*/
- //pc.printf("POWER ON\r\n");
- twe.printf("POWER ON\r\n");
- /*入力待ち*/
- while(1){
- char cha = twe.getc();
- if(_input(cha)==-1){
- timer_log.start();
- tic_log.attach(&_log, 1.0/RATE_LOG);
- break;
- }
- }
- /*フライト準備*/
- Phase = SETUP;
- tic_open.attach(&_open, 1.0/RATE_OPEN);
+
+ /*初期位置の設定*/
+ mpu.getAccelero(acc);
+ mpu.getGyro(gyro);
+ degRoll = atan2(acc[1], acc[2]) * RadToDeg;
+ degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
+ gKfx.setAngle(degRoll);
+ gKfy.setAngle(degPitch);
+ gCalibrateY = degPitch;
+ gCalibrateX = degRoll;
+
+ Phase = STANDBY;
- /*GPS*/
- while(1){
- /*GPS取得*/
- if(Phase != RISE){
- 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);
- }else{
- twe.printf("%s\r\n",gps_data);
- }
- }
- }else{
- cnt_gps++;
+ while(1){
+ switch(Phase){
+ case STANDBY:
+ /*入力待ち*/
+ timer_data.start();
+ tic_data.attach(&_SendData, 1.0/RATE_DATA);
+ Phase = LAUNCH;
+ break;
+ case LAUNCH:
+ for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
+ mpu.getAccelero(acc);
+ 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(acc_abs>ACC_JUDGE_LAUNCH){
+ cnt_judge++;
}
- }
- }
-
- if(timer_log.read()>=30.0*60.0){
- timer_log.reset();
- /*なんかずっとこればっか書き込まれた 2018/7/1
- fp=fopen("/sd/mydir/data.txt","w");
- fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
- fclose(fp);
- */
- }
- }
-}
-
-void _open(){
- led = Phase;
- float acc[3],acc_buf[10],acc_abs,time_acc,time_alt;
- switch(Phase){
- case SETUP: //地上高度取得
- //pc.printf("Hello,World!\r\n");
- twe.printf("Hello,World!\r\n");
- for(Cnt=0;Cnt<10;Cnt++){
- Alt_buf[Cnt] = _getAlt();
- }
- Alt_gnd = _median(Alt_buf,10);
- /*データをローカルファイルに保存*/
- lfp=fopen("/local/data.txt", "a");
- fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
- fclose(lfp);
-
- timer_open.start();
- Phase = LAUNCH;
- //pc.printf("Phase = LAUNCH\r\n");
- twe.printf("Phase = LAUNCH\r\n");
- break;
-
- case LAUNCH: //発射判定
- for(Cnt=0;Cnt<10;Cnt++){
+ if(cnt_judge==CNT_JUDGE){
+ cnt_judge=0;
+ timer_open.start();
+ Phase = FIRE;
+ }
+ break;
+ case RISE:
+ while(timer_open.read() < TIME_BURNING){
+ }
+ for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
+ alt_buff[cnt_data] = _getAlt();
+ }
+ alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
+ if(alt_md > ALT_JUDGE_FIRE){
+ Phase = FIRE;
+ }
+ break;
+ case FIRE:
+ gPrevMicros = timer_open.read();
mpu.getAccelero(acc);
- acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2));
- }
- acc_abs = _median(acc_buf,10);
- //pc.printf("acc:%f\r\n",acc_abs);
- //pc.printf("cnt:%d\r\n",Cnt_acc);
- //加速度判定
- if(acc_abs>ACC_JUDGE_LAUNCH){
- Cnt_acc++;
- time_acc = timer_open.read();
- }
- if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0;
- if(Cnt_acc == CNT_JUDGE_LAUNCH){
- //pc.printf("Phase = WAIT\r\n");
- twe.printf("LAUNCHED\r\n");
- timer_open.reset();
- Phase = WAIT;
- }
- break;
-
- case WAIT: //待機時間
- if(timer_open.read()>TIME_GAP) Phase = RISE;
- break;
-
- case RISE: //頂点判定
- for(Cnt=0;Cnt<5;Cnt++){
- Alt_buf[Cnt] = _getAlt();
- //pc.printf("alt:%f\r\n",Alt_buf[Cnt]);
- }
- Alt_md = _median(Alt_buf,5);
- twe.printf("alt:%f\r\n",Alt_md);
- //twe.printf("cnt:%d\r\n",Cnt_alt);
- if(Alt_md > Alt_max){
- Alt_max = Alt_md;
- Cnt_alt = 0;
- }
- else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){
- Cnt_alt++;
- Time_alt = timer_open.read();
- }
- if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){
- Cnt_alt = 0;
- }
- if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){
- //pc.printf("Phase = DROP1\r\n");
- twe.printf("OPEN PARA\r\n");
- Cnt_alt = 0;
- /*ローカルファイルに最高高度を記録*/
- lfp = fopen("/local/data.txt", "a");
- fprintf(lfp,"最高高度：%f\r\n",Alt_max);
- fclose(lfp);
+ mpu.getGyro(gyro);
+ degRoll = atan2(acc[1], acc[2]) * RadToDeg;
+ degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
+
+ float dpsX = gyro[0] * RadToDeg;
+ float dpsY = gyro[1] * RadToDeg;
+ float dpsZ = gyro[2] * RadToDeg;
+
+ float curMicros = timer_open.read();
+ float dt = curMicros - gPrevMicros;
+ gPrevMicros = curMicros;
- servo1.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
- servo2.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
- Phase = DROP1;
- }
- break;
+ float degX = gKfx.calcAngle(degRoll, dpsX, dt);
+ float degY = gKfy.calcAngle(degPitch, dpsY, dt);
+ degY -= gCalibrateY;
+ degX -= gCalibrateX;
+ if(degY>ANGLE_JUDGE_FIRE){
+ Phase = OPEN;
+ }
+ 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);
+ if(alt_md > alt_max){
+ alt_max = alt_md;
+ cnt_judge = 0;
+ }
+ else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
+ cnt_judge++;
+ time_judge = timer_open.read();
+ }
- case DROP1: //放出機構作動？
- for(Cnt=0;Cnt<5;Cnt++){
- Alt_buf[Cnt] = _getAlt();
- }
- Alt_md = _median(Alt_buf,5);
- twe.printf("Alt:%f\r\n",Alt_md);
- //twe.printf("Cnt:%d\r\n",Cnt_alt);
- if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){
- Cnt_alt++;
- }
- if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする？到達高度低いかも？
- //pc.printf("Phase = DROP2\r\n");
- twe.printf("GO\r\n");
- servo3.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
- servo4.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
- Phase = DROP2;
- }
- break;
-
- case DROP2: //着水検知．サーボの電源落とす．
- //twe.printf("cnt:%d\r\n",Cnt_alt);
- wait(3.0);
- sw = 0;
- //pc.printf("Goodbye Servo\r\n");
- twe.printf("FINISH\r\n");
- Phase=SEA;
- break;
- }
-}
-
-void _log(){
- Data[Row][Cnt_data][0]=Phase;
- Data[Row][Cnt_data][1]=timer_log.read();
- mpu.getAccelero(&Data[Row][Cnt_data][2]);
- bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]);
- Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f;
- Cnt_data++;
- //pc.printf("Cnt_data:%d\r\n",Cnt_data);
- if(Cnt_data==NUM_DATA){
- Cnt_data = 0;
- Row =! Row;
- fp = fopen("/sd/mydir/data.txt","a");
- for(int i=0;i<NUM_DATA;i++){
- fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]);
+ if((timer_open.read()-time_judge) - TIME_JUDGE_CNT) cnt_judge=0;
+ if(cnt_judge == CNT_JUDGE){
+ cnt_judge = 0;
+ Phase = RECOVERY;
+ }
+ break;
+ case RECOVERY:
+ tic_data.detach();
+ //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS);
+ Phase = SEA;
+ break;
+ case SEA:
+ break;
}
- fclose(fp);
}
}
@@ -278,28 +164,75 @@
return altitude;
}
-int _input(char cha){
- twe.printf("%c\r\n",cha);
- if(cha!='\0'){
-// pc.printf("%c\r\n",c);
- }
- if(cha=='F'){
- twe.printf("flight mode on\r\n");
- return -1;
- }else if(cha=='u'){ //開放機構のUNLOCK
- servo1.pulsewidth(0.0006);
- servo2.pulsewidth(0.0006);
- }else if(cha=='l'){ //開放機構のLOCK
- servo1.pulsewidth(0.0024);
- servo2.pulsewidth(0.0024);
- }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU)
- servo3.pulsewidth(0.0010);
- servo4.pulsewidth(0.0010);
- }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU)
- servo3.pulsewidth(0.0024);
- servo4.pulsewidth(0.0024);
+void _SendData(){
+ float pretime,a[3],g[3],alt;
+ //カルマン
+ /*
+ pretime = timer_data.read();
+ mpu.getAccelero(a);
+ mpu.getGyro(g);
+ float degroll = atan2(a[1], a[2]) * RadToDeg;
+ float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
+
+ float dpsx = g[0] * RadToDeg;
+ float dpsy = g[1] * RadToDeg;
+ float dpsz = g[2] * RadToDeg;
+
+ float curtime = timer_data.read();
+ float t = curtime - pretime;
+ pretime = curtime;
+
+ float degx = gKfx.calcAngle(degroll, dpsx, t);
+ float degy = gKfy.calcAngle(degpitch, dpsx, t);
+ degy -= gCalibrateY;
+ degx -= gCalibrateX;
+ */
+ /*madgwick*/
+ float Roll,Pitch,Yaw;
+ MadgwickFilter.begin(2);
+ mpu.getAccelero(a);
+ mpu.getGyro(g);
+ g[0] *= RadToDeg;
+ g[1] *= RadToDeg;
+ g[2] *= RadToDeg;
+
+ MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]);
+ Roll = MadgwickFilter.getRoll();
+ Pitch = MadgwickFilter.getPitch();
+ Yaw = MadgwickFilter.getYaw();
+
+ alt = _getAlt();
+
+ //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);
+ }else{
+ twe.printf("%s\r\n",gps_data);
+ }
+ }
+ }else{
+ cnt_gps++;
+ }
}
- return 0;
}
float _DMS2DEG(float raw_data){
@@ -307,6 +240,7 @@
float m=(raw_data-(float)d*100);
return (float)d+m/60;
}
+*/
float _median(float data[], int num){
float *data_cpy, ans;
@@ -327,4 +261,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