Haruki Sashida / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@6:f17310205c1f, 2019-02-12 (annotated)

Committer:
Yukina
Date:
Tue Feb 12 11:20:33 2019 +0000
Revision:
6:f17310205c1f
Parent:
5:f6e956e8a060
Child:
7:9953d922499d
add madgwick

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:0ff20d8e9090 1 #include "mbed.h"
Yukina 4:4b3ae90ec778 2 #include "MPU6050.h"
mikawataru 0:0ff20d8e9090 3 #include "BMP180.h"
Yukina 5:f6e956e8a060 4 #include "Kalman.h"
Yukina 5:f6e956e8a060 5 #include "MadgwickAHRS.h"
mikawataru 0:0ff20d8e9090 6
Yukina 5:f6e956e8a060 7 /*しきい値など*/
Yukina 5:f6e956e8a060 8 #define ACC_JUDGE_LAUNCH 1.5
Yukina 5:f6e956e8a060 9 #define TIME_BURNING 6
Yukina 5:f6e956e8a060 10 #define ALT_JUDGE_FIRE 1
Yukina 5:f6e956e8a060 11 #define ALT_JUDGE_OPEN 2
Yukina 5:f6e956e8a060 12 #define ANGLE_JUDGE_FIRE 90
Yukina 5:f6e956e8a060 13 #define CNT_JUDGE 3
Yukina 5:f6e956e8a060 14 #define TIME_JUDGE_CNT 1.5
Yukina 5:f6e956e8a060 15 #define NUM_CNT_MEDIAN 10
Yukina 5:f6e956e8a060 16 #define RATE_GPS 1
Yukina 5:f6e956e8a060 17 #define RATE_DATA 2
Yukina 5:f6e956e8a060 18 #define p0 1013.25f
Yukina 5:f6e956e8a060 19 #define RadToDeg 57.295779513082320876798154814105
Yukina 5:f6e956e8a060 20
Yukina 5:f6e956e8a060 21 MPU6050 mpu(dp5,dp27);
Yukina 5:f6e956e8a060 22 BMP180 bmp(dp5,dp27);
Yukina 5:f6e956e8a060 23 KalmanFilter gKfx, gKfy;
Yukina 5:f6e956e8a060 24 Madgwick MadgwickFilter;
Yukina 5:f6e956e8a060 25 Serial pc(dp16,dp15);
Yukina 5:f6e956e8a060 26 Timer timer_open;
Yukina 5:f6e956e8a060 27 Timer timer_data;
Yukina 5:f6e956e8a060 28 Ticker tic_data;
Yukina 5:f6e956e8a060 29 Ticker tic_gps;
mikawataru 0:0ff20d8e9090 30
Yukina 5:f6e956e8a060 31 /*自作関数*/
Yukina 5:f6e956e8a060 32 float _getAlt();
Yukina 5:f6e956e8a060 33 float _median(float data[],int num);
Yukina 5:f6e956e8a060 34 void _SendData();
Yukina 5:f6e956e8a060 35 void _SendGPS();
Yukina 5:f6e956e8a060 36 float _DMS2DEG(float raw_data);
Yukina 4:4b3ae90ec778 37
Yukina 5:f6e956e8a060 38 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
Yukina 4:4b3ae90ec778 39
Yukina 5:f6e956e8a060 40 /*グローバル変数*/
Yukina 5:f6e956e8a060 41 //カルマン用
Yukina 5:f6e956e8a060 42 float gCalibrateX;
Yukina 5:f6e956e8a060 43 float gCalibrateY;
Yukina 5:f6e956e8a060 44 float gPrevMicros;
Yukina 5:f6e956e8a060 45 float degRoll,degPitch;
mikawataru 0:0ff20d8e9090 46
Yukina 4:4b3ae90ec778 47 int main(){
Yukina 5:f6e956e8a060 48 /*ローカル変数*/
Yukina 5:f6e956e8a060 49 float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
Yukina 5:f6e956e8a060 50 float alt_buff[10],alt_md,alt_max;
Yukina 5:f6e956e8a060 51 float time_judge;
Yukina 5:f6e956e8a060 52 int cnt_data=0,cnt_judge=0;
Yukina 5:f6e956e8a060 53
Yukina 5:f6e956e8a060 54 /*センサの初期化等*/
Yukina 5:f6e956e8a060 55 pc.baud(9600);
Yukina 5:f6e956e8a060 56 mpu.setAcceleroRange(3);
mikawataru 0:0ff20d8e9090 57 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
Yukina 5:f6e956e8a060 58
Yukina 5:f6e956e8a060 59 /*初期位置の設定*/
Yukina 5:f6e956e8a060 60 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 61 mpu.getGyro(gyro);
Yukina 5:f6e956e8a060 62 degRoll = atan2(acc[1], acc[2]) * RadToDeg;
Yukina 5:f6e956e8a060 63 degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
Yukina 5:f6e956e8a060 64 gKfx.setAngle(degRoll);
Yukina 5:f6e956e8a060 65 gKfy.setAngle(degPitch);
Yukina 5:f6e956e8a060 66 gCalibrateY = degPitch;
Yukina 5:f6e956e8a060 67 gCalibrateX = degRoll;
Yukina 5:f6e956e8a060 68
Yukina 5:f6e956e8a060 69 Phase = STANDBY;
Yukina 4:4b3ae90ec778 70
Yukina 5:f6e956e8a060 71 while(1){
Yukina 5:f6e956e8a060 72 switch(Phase){
Yukina 5:f6e956e8a060 73 case STANDBY:
Yukina 5:f6e956e8a060 74 /*入力待ち*/
Yukina 5:f6e956e8a060 75 timer_data.start();
Yukina 5:f6e956e8a060 76 tic_data.attach(&_SendData, 1.0/RATE_DATA);
Yukina 5:f6e956e8a060 77 Phase = LAUNCH;
Yukina 5:f6e956e8a060 78 break;
Yukina 5:f6e956e8a060 79 case LAUNCH:
Yukina 5:f6e956e8a060 80 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 81 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 82 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));
Yukina 5:f6e956e8a060 83 }
Yukina 5:f6e956e8a060 84 acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
Yukina 5:f6e956e8a060 85 /*加速度判定*/
Yukina 5:f6e956e8a060 86 if(acc_abs>ACC_JUDGE_LAUNCH){
Yukina 5:f6e956e8a060 87 cnt_judge++;
Yukina 4:4b3ae90ec778 88 }
Yukina 5:f6e956e8a060 89 if(cnt_judge==CNT_JUDGE){
Yukina 5:f6e956e8a060 90 cnt_judge=0;
Yukina 5:f6e956e8a060 91 timer_open.start();
Yukina 5:f6e956e8a060 92 Phase = FIRE;
Yukina 5:f6e956e8a060 93 }
Yukina 5:f6e956e8a060 94 break;
Yukina 5:f6e956e8a060 95 case RISE:
Yukina 5:f6e956e8a060 96 while(timer_open.read() < TIME_BURNING){
Yukina 5:f6e956e8a060 97 }
Yukina 5:f6e956e8a060 98 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 99 alt_buff[cnt_data] = _getAlt();
Yukina 5:f6e956e8a060 100 }
Yukina 5:f6e956e8a060 101 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
Yukina 5:f6e956e8a060 102 if(alt_md > ALT_JUDGE_FIRE){
Yukina 5:f6e956e8a060 103 Phase = FIRE;
Yukina 5:f6e956e8a060 104 }
Yukina 5:f6e956e8a060 105 break;
Yukina 5:f6e956e8a060 106 case FIRE:
Yukina 6:f17310205c1f 107
Yukina 6:f17310205c1f 108 //カルマン
Yukina 6:f17310205c1f 109 /*
Yukina 5:f6e956e8a060 110 gPrevMicros = timer_open.read();
Yukina 4:4b3ae90ec778 111 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 112 mpu.getGyro(gyro);
Yukina 5:f6e956e8a060 113 degRoll = atan2(acc[1], acc[2]) * RadToDeg;
Yukina 5:f6e956e8a060 114 degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
Yukina 5:f6e956e8a060 115
Yukina 5:f6e956e8a060 116 float dpsX = gyro[0] * RadToDeg;
Yukina 5:f6e956e8a060 117 float dpsY = gyro[1] * RadToDeg;
Yukina 5:f6e956e8a060 118 float dpsZ = gyro[2] * RadToDeg;
Yukina 5:f6e956e8a060 119
Yukina 5:f6e956e8a060 120 float curMicros = timer_open.read();
Yukina 5:f6e956e8a060 121 float dt = curMicros - gPrevMicros;
Yukina 5:f6e956e8a060 122 gPrevMicros = curMicros;
Yukina 4:4b3ae90ec778 123
Yukina 5:f6e956e8a060 124 float degX = gKfx.calcAngle(degRoll, dpsX, dt);
Yukina 5:f6e956e8a060 125 float degY = gKfy.calcAngle(degPitch, dpsY, dt);
Yukina 5:f6e956e8a060 126 degY -= gCalibrateY;
Yukina 5:f6e956e8a060 127 degX -= gCalibrateX;
Yukina 5:f6e956e8a060 128 if(degY>ANGLE_JUDGE_FIRE){
Yukina 5:f6e956e8a060 129 Phase = OPEN;
Yukina 5:f6e956e8a060 130 }
Yukina 6:f17310205c1f 131 */
Yukina 6:f17310205c1f 132
Yukina 6:f17310205c1f 133 //madgwick
Yukina 6:f17310205c1f 134 float Roll,Pitch,Yaw;
Yukina 6:f17310205c1f 135 MadgwickFilter.begin(2);
Yukina 6:f17310205c1f 136 mpu.getAccelero(acc);
Yukina 6:f17310205c1f 137 mpu.getGyro(gyro);
Yukina 6:f17310205c1f 138 gyro[0] *= RadToDeg;
Yukina 6:f17310205c1f 139 gyro[1] *= RadToDeg;
Yukina 6:f17310205c1f 140 gyro[2] *= RadToDeg;
Yukina 6:f17310205c1f 141
Yukina 6:f17310205c1f 142 MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]);
Yukina 6:f17310205c1f 143 Roll = MadgwickFilter.getRoll();
Yukina 6:f17310205c1f 144 Pitch = MadgwickFilter.getPitch();
Yukina 6:f17310205c1f 145 Yaw = MadgwickFilter.getYaw();
Yukina 6:f17310205c1f 146 if(Pitch>ANGLE_JUDGE_FIRE){
Yukina 6:f17310205c1f 147 Phase = OPEN;
Yukina 6:f17310205c1f 148 }
Yukina 6:f17310205c1f 149
Yukina 5:f6e956e8a060 150 break;
Yukina 5:f6e956e8a060 151 case OPEN:
Yukina 5:f6e956e8a060 152 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 153 alt_buff[cnt_data] = _getAlt();
Yukina 5:f6e956e8a060 154 }
Yukina 5:f6e956e8a060 155 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
Yukina 5:f6e956e8a060 156 if(alt_md > alt_max){
Yukina 5:f6e956e8a060 157 alt_max = alt_md;
Yukina 5:f6e956e8a060 158 cnt_judge = 0;
Yukina 5:f6e956e8a060 159 }
Yukina 5:f6e956e8a060 160 else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
Yukina 5:f6e956e8a060 161 cnt_judge++;
Yukina 5:f6e956e8a060 162 time_judge = timer_open.read();
Yukina 5:f6e956e8a060 163 }
Yukina 4:4b3ae90ec778 164
Yukina 5:f6e956e8a060 165 if((timer_open.read()-time_judge) - TIME_JUDGE_CNT) cnt_judge=0;
Yukina 5:f6e956e8a060 166 if(cnt_judge == CNT_JUDGE){
Yukina 5:f6e956e8a060 167 cnt_judge = 0;
Yukina 5:f6e956e8a060 168 Phase = RECOVERY;
Yukina 5:f6e956e8a060 169 }
Yukina 5:f6e956e8a060 170 break;
Yukina 5:f6e956e8a060 171 case RECOVERY:
Yukina 5:f6e956e8a060 172 tic_data.detach();
Yukina 5:f6e956e8a060 173 //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS);
Yukina 5:f6e956e8a060 174 Phase = SEA;
Yukina 5:f6e956e8a060 175 break;
Yukina 5:f6e956e8a060 176 case SEA:
Yukina 5:f6e956e8a060 177 break;
Yukina 4:4b3ae90ec778 178 }
Yukina 4:4b3ae90ec778 179 }
Yukina 4:4b3ae90ec778 180 }
Yukina 4:4b3ae90ec778 181
Yukina 4:4b3ae90ec778 182 float _getAlt(){
Yukina 4:4b3ae90ec778 183 float altitude,pressure,temperature;
Yukina 4:4b3ae90ec778 184 bmp.ReadData(&temperature,&pressure);
Yukina 4:4b3ae90ec778 185 altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
Yukina 4:4b3ae90ec778 186 return altitude;
Yukina 4:4b3ae90ec778 187 }
Yukina 4:4b3ae90ec778 188
Yukina 5:f6e956e8a060 189 void _SendData(){
Yukina 5:f6e956e8a060 190 float pretime,a[3],g[3],alt;
Yukina 5:f6e956e8a060 191 //カルマン
Yukina 5:f6e956e8a060 192 /*
Yukina 5:f6e956e8a060 193 pretime = timer_data.read();
Yukina 5:f6e956e8a060 194 mpu.getAccelero(a);
Yukina 5:f6e956e8a060 195 mpu.getGyro(g);
Yukina 5:f6e956e8a060 196 float degroll = atan2(a[1], a[2]) * RadToDeg;
Yukina 5:f6e956e8a060 197 float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
Yukina 5:f6e956e8a060 198
Yukina 5:f6e956e8a060 199 float dpsx = g[0] * RadToDeg;
Yukina 5:f6e956e8a060 200 float dpsy = g[1] * RadToDeg;
Yukina 5:f6e956e8a060 201 float dpsz = g[2] * RadToDeg;
Yukina 5:f6e956e8a060 202
Yukina 5:f6e956e8a060 203 float curtime = timer_data.read();
Yukina 5:f6e956e8a060 204 float t = curtime - pretime;
Yukina 5:f6e956e8a060 205 pretime = curtime;
Yukina 5:f6e956e8a060 206
Yukina 5:f6e956e8a060 207 float degx = gKfx.calcAngle(degroll, dpsx, t);
Yukina 5:f6e956e8a060 208 float degy = gKfy.calcAngle(degpitch, dpsx, t);
Yukina 5:f6e956e8a060 209 degy -= gCalibrateY;
Yukina 5:f6e956e8a060 210 degx -= gCalibrateX;
Yukina 5:f6e956e8a060 211 */
Yukina 5:f6e956e8a060 212 /*madgwick*/
Yukina 5:f6e956e8a060 213 float Roll,Pitch,Yaw;
Yukina 5:f6e956e8a060 214 MadgwickFilter.begin(2);
Yukina 5:f6e956e8a060 215 mpu.getAccelero(a);
Yukina 5:f6e956e8a060 216 mpu.getGyro(g);
Yukina 5:f6e956e8a060 217 g[0] *= RadToDeg;
Yukina 5:f6e956e8a060 218 g[1] *= RadToDeg;
Yukina 5:f6e956e8a060 219 g[2] *= RadToDeg;
Yukina 5:f6e956e8a060 220
Yukina 5:f6e956e8a060 221 MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]);
Yukina 5:f6e956e8a060 222 Roll = MadgwickFilter.getRoll();
Yukina 5:f6e956e8a060 223 Pitch = MadgwickFilter.getPitch();
Yukina 5:f6e956e8a060 224 Yaw = MadgwickFilter.getYaw();
Yukina 5:f6e956e8a060 225
Yukina 5:f6e956e8a060 226 alt = _getAlt();
Yukina 5:f6e956e8a060 227
Yukina 5:f6e956e8a060 228 //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy);
Yukina 5:f6e956e8a060 229 pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
Yukina 5:f6e956e8a060 230 }
Yukina 5:f6e956e8a060 231
Yukina 5:f6e956e8a060 232 /*
Yukina 5:f6e956e8a060 233 void _SendGPS(){
Yukina 5:f6e956e8a060 234 int cnt_gps;
Yukina 5:f6e956e8a060 235 if(gps.readable()){
Yukina 5:f6e956e8a060 236 gps_data[cnt_gps] = gps.getc();
Yukina 5:f6e956e8a060 237 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
Yukina 5:f6e956e8a060 238 cnt_gps = 0;
Yukina 5:f6e956e8a060 239 memset(gps_data,'\0',256);
Yukina 5:f6e956e8a060 240 }else if(gps_data[cnt_gps] == '\r'){
Yukina 5:f6e956e8a060 241 float world_time, lon_east, lat_north;
Yukina 5:f6e956e8a060 242 int rlock, sat_num;
Yukina 5:f6e956e8a060 243 char lat,lon;
Yukina 5:f6e956e8a060 244 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
Yukina 5:f6e956e8a060 245 if(rlock==1){
Yukina 5:f6e956e8a060 246 lat_north = _DMS2DEG(lat_north);
Yukina 5:f6e956e8a060 247 lon_east = _DMS2DEG(lon_east);
Yukina 5:f6e956e8a060 248 twe.printf("%s\r\n",gps_data);
Yukina 5:f6e956e8a060 249 twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
Yukina 5:f6e956e8a060 250 }else{
Yukina 5:f6e956e8a060 251 twe.printf("%s\r\n",gps_data);
Yukina 5:f6e956e8a060 252 }
Yukina 5:f6e956e8a060 253 }
Yukina 5:f6e956e8a060 254 }else{
Yukina 5:f6e956e8a060 255 cnt_gps++;
Yukina 5:f6e956e8a060 256 }
Yukina 4:4b3ae90ec778 257 }
Yukina 4:4b3ae90ec778 258 }
Yukina 4:4b3ae90ec778 259
Yukina 4:4b3ae90ec778 260 float _DMS2DEG(float raw_data){
Yukina 4:4b3ae90ec778 261 int d=(int)(raw_data/100);
Yukina 4:4b3ae90ec778 262 float m=(raw_data-(float)d*100);
Yukina 4:4b3ae90ec778 263 return (float)d+m/60;
Yukina 4:4b3ae90ec778 264 }
Yukina 5:f6e956e8a060 265 */
Yukina 4:4b3ae90ec778 266
Yukina 4:4b3ae90ec778 267 float _median(float data[], int num){
Yukina 4:4b3ae90ec778 268 float *data_cpy, ans;
Yukina 4:4b3ae90ec778 269 data_cpy = new float[num];
Yukina 4:4b3ae90ec778 270 memcpy(data_cpy,data,sizeof(float)*num);
Yukina 4:4b3ae90ec778 271
Yukina 4:4b3ae90ec778 272 for(int i=0; i<num; i++){
Yukina 4:4b3ae90ec778 273 for(int j=0; j<num-i-1; j++){
Yukina 4:4b3ae90ec778 274 if(data_cpy[j]>data_cpy[j+1]){
Yukina 4:4b3ae90ec778 275 float buff = data_cpy[j+1];
Yukina 4:4b3ae90ec778 276 data_cpy[j+1] = data_cpy[j];
Yukina 4:4b3ae90ec778 277 data_cpy[j] = buff;
Yukina 4:4b3ae90ec778 278 }
Yukina 4:4b3ae90ec778 279 }
Yukina 4:4b3ae90ec778 280 }
Yukina 4:4b3ae90ec778 281
Yukina 4:4b3ae90ec778 282 if(num%2!=0) ans = data_cpy[num/2];
Yukina 4:4b3ae90ec778 283 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
Yukina 4:4b3ae90ec778 284 delete[] data_cpy;
Yukina 4:4b3ae90ec778 285 return ans;
Yukina 5:f6e956e8a060 286 }