201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 11:c90db1500720
- Parent:
- 10:1a626929850e
- Child:
- 12:6a3c0e9075eb
diff -r 1a626929850e -r c90db1500720 main.cpp --- a/main.cpp Sun Mar 10 03:41:56 2019 +0000 +++ b/main.cpp Sun Mar 10 08:17:44 2019 +0000 @@ -1,66 +1,46 @@ #include "mbed.h" #include "MPU6050.h" #include "BMP180.h" -#include "Kalman.h" -#include "MadgwickAHRS.h" /*しきい値など*/ -#define ACC_JUDGE_LAUNCH 3.0 //発射判定のしきい値 +#define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値 #define TIME_BURNING 6 //開放判定しない時間(燃焼時間) -#define ALT_JUDGE_FIRE 0 #define ALT_JUDGE_OPEN 1 //落下判定のカウントを1増やす高度差 #define TIME_OPEN 25 //強制的に開放させる時間 -#define TIME_SEND 1.0 -#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.0 -#define RATE_DATA 10 -#define TIMER_NOTFIRE 15.0 -#define p0 1013.25f -#define RadToDeg 57.295779513082320876798154814105 +#define TIME_SEND 1.0 //無線送信する間隔 +#define CNT_JUDGE 10 //頂点判定する時に落下のカウント数 +#define NUM_CNT_MEDIAN 10 //中央値をとる個数 +#define RATE_GPS 1.0 //GPSのTickerを動作させるタイミング +#define p0 1013.25f //海面気圧 MPU6050 mpu(PB_7,PB_6); BMP180 bmp(PB_7,PB_6); -KalmanFilter gKfx, gKfy; -Madgwick MadgwickFilter; Serial pc(PA_2, PA_3); Serial gps(PA_9, PA_10); DigitalOut myled(PA_15); DigitalOut ES920_RST(PA_5); +PwmOut servo1(PB_4); +PwmOut servo2(PB_5); +DigitalOut servo1_signal(PA_0); +DigitalOut servo2_signal(PA_1); Timer timer_open; Timer timer_data; -Ticker tic_data; Ticker tic_gps; /*自作関数*/ float _getAlt(); float _median(float data[],int num); -void _SendData(); void _SendGPS(); float _DMS2DEG(float raw_data); -int servo(); enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; /*グローバル変数*/ -float t = 0; //地上高度 float alt_gnd; float alt_max; -//GPS -int cnt_gps=0; - -int p = 1; -//サーボ +char c[3]; int i = 0; -char c[516]; -char d [8]; -int len; -char f[]="Receive"; - void main(){ ES920_RST = 0; myled =0; @@ -69,7 +49,7 @@ float alt_buff[10],alt_md; float time_judge; int cnt_data=0,cnt_judge=0; - char gps_data[256]; + float t = 0; /*センサの初期化等*/ pc.baud(38400); mpu.setAcceleroRange(3); @@ -90,17 +70,35 @@ pc.printf("f:Flight_mode_on\r\n"); wait(1.0); timer_data.start(); + servo1_signal = 1; + servo2_signal = 1; + servo1.pulsewidth(0.0024); + servo1.pulsewidth(0.0024); while(1){ switch(Phase){ case STANDBY: /*サーボ入力待ち*/ //servo(); c[0]=pc.getc(); - if(c[0] == 'o') myled = 1; - if(c[0] == 'l') myled = 0; + if(c[0] == 'o'){ + myled = 1; + servo1.pulsewidth(0.0006); + servo2.pulsewidth(0.0006); + mpu.getAccelero(acc); + 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))); + } + if(c[0] == 'l'){ + myled = 0; + servo1.pulsewidth(0.0024); + servo2.pulsewidth(0.0024); + mpu.getAccelero(acc); + 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))); + } if(c[0] == 'f'){ //pc.printf("flight_mode\r\n"); Phase = LAUNCH; + servo1_signal = 0; + servo2_signal = 0; } break; @@ -119,6 +117,8 @@ cnt_judge++; } if(cnt_judge==CNT_JUDGE){ + servo1_signal = 1; + servo1_signal = 1; cnt_judge=0; timer_open.start(); Phase = RISE; @@ -136,62 +136,6 @@ t = 0.0; break; case FIRE: - - //カルマン - /* - gPrevMicros = timer_open.read(); - 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; - - 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; - - 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; - } - */ - - //madgwick - float Roll,Pitch,Yaw; - MadgwickFilter.begin(2); - mpu.getAccelero(acc); - mpu.getGyro(gyro); - gyro[0] *= RadToDeg; - gyro[1] *= RadToDeg; - gyro[2] *= RadToDeg; - - MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); - Roll = MadgwickFilter.getRoll(); - Pitch = MadgwickFilter.getPitch(); - Yaw = MadgwickFilter.getYaw(); - 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++){ @@ -232,49 +176,6 @@ return altitude; } -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(); - 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); -} - float _DMS2DEG(float raw_data){ int d=(int)(raw_data/100); float m=(raw_data-(float)d*100); @@ -303,48 +204,9 @@ return ans; } -int servo(void){ - do{ - c[i] = pc.getc(); - gps.printf("%c",c[i]); - i++; - }while(c[i-1] != '\n'); - gps.printf("path\r\n"); - //gps.printf("%s",c); - myled = 1; - //gps.printf("%s",c); - len = strlen(f); - i=0; - while(i != 4){ - if (c[i] != f[i]) break; - i++; - } - if(i > 3){ - i=0; - do{ - d[i]=c[13+i]; - i++; - }while(d[i-1] != ')'); - d[i-1] = '\0'; - gps.printf("d:%s",d); - i = 0; - if(d[0] == 'o') myled = 1; - if(d[0] == 'l') myled = 0; - if(d[0] == 'f'){ - //pc.printf("flight_mode\r\n"); - Phase = LAUNCH; - } - } - - i=0; - memset(c,'\0',64); - memset(d,'\0',8); - //pc.printf("2\r\n"); - return 0; -} - void _SendGPS(){ char gps_data[256]; + int cnt_gps=0; while(1){ if(gps.readable()){ gps_data[cnt_gps] = gps.getc();