201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 5:f6e956e8a060
- Parent:
- 4:4b3ae90ec778
- Child:
- 6:f17310205c1f
diff -r 4b3ae90ec778 -r f6e956e8a060 main.cpp --- 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