201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp
- Committer:
- Yukina
- Date:
- 2018-12-30
- Revision:
- 5:f6e956e8a060
- Parent:
- 4:4b3ae90ec778
- Child:
- 6:f17310205c1f
File content as of revision 5:f6e956e8a060:
#include "mbed.h" #include "MPU6050.h" #include "BMP180.h" #include "Kalman.h" #include "MadgwickAHRS.h" /*しきい値など*/ #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; /*自作関数*/ float _getAlt(); float _median(float data[],int num); void _SendData(); void _SendGPS(); float _DMS2DEG(float raw_data); enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; /*グローバル変数*/ //カルマン用 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); /*初期位置の設定*/ 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; 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(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); 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; } 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(); } 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; } } } float _getAlt(){ float altitude,pressure,temperature; bmp.ReadData(&temperature,&pressure); altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; 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(); //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++; } } } float _DMS2DEG(float raw_data){ int d=(int)(raw_data/100); float m=(raw_data-(float)d*100); return (float)d+m/60; } */ float _median(float data[], int num){ float *data_cpy, ans; data_cpy = new float[num]; memcpy(data_cpy,data,sizeof(float)*num); for(int i=0; i<num; i++){ for(int j=0; j<num-i-1; j++){ if(data_cpy[j]>data_cpy[j+1]){ float buff = data_cpy[j+1]; data_cpy[j+1] = data_cpy[j]; data_cpy[j] = buff; } } } if(num%2!=0) ans = data_cpy[num/2]; else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; delete[] data_cpy; return ans; }