CORE / Mbed 2 deprecated Misora

能代宇宙イベント2020,ロケット用プログラム Nucleo-F303K8 ,MPU9250,BME280

Dependencies:   mbed SDFileSystem BME280 MPU9250

main.cpp@0:aa715ea672cb, 2020-10-29 (annotated)

Committer:
Zero0000
Date:
Thu Oct 29 10:23:16 2020 +0000
Revision:
0:aa715ea672cb
rocket

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zero0000 0:aa715ea672cb 1 #include "mbed.h"
Zero0000 0:aa715ea672cb 2 #include "math.h"
Zero0000 0:aa715ea672cb 3 #include "MPU9250.h"
Zero0000 0:aa715ea672cb 4 #include "BME280.h"
Zero0000 0:aa715ea672cb 5 #include "SDFileSystem.h"
Zero0000 0:aa715ea672cb 6 #define NUM_CNT_MEDIAN 10 //中央値をとる個数
Zero0000 0:aa715ea672cb 7 #define CNT_LAUNCH_TIMES 5//発射判定に必要な連続数
Zero0000 0:aa715ea672cb 8 #define TIME_SEND 1.0 //無線送信する間隔
Zero0000 0:aa715ea672cb 9 #define CNT_LAUNCH 5 //発射判定するときのしきい値(TBD)
Zero0000 0:aa715ea672cb 10 #define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値
Zero0000 0:aa715ea672cb 11 #define TIME_BURNING 6 //燃焼時間(TBD)
Zero0000 0:aa715ea672cb 12 #define ALT_JUDGE_OPEN 5 //落下判定のカウントを１増やす高度差
Zero0000 0:aa715ea672cb 13 #define CNT_JUDGE 10 //頂点判定する時に落下のカウント数
Zero0000 0:aa715ea672cb 14 #define p0 1013.25f //海面気圧
Zero0000 0:aa715ea672cb 15 #define N 5
Zero0000 0:aa715ea672cb 16 #define sampleFreq 100.0f
Zero0000 0:aa715ea672cb 17 #define beta 0.33f // gain（大きいと加速度による補正が早い）
Zero0000 0:aa715ea672cb 18 #define PI 3.14159265358979323846f
Zero0000 0:aa715ea672cb 19
Zero0000 0:aa715ea672cb 20 MPU9250 mpu = MPU9250(PB_7,PB_6);// pin30,29 SDA,SCL
Zero0000 0:aa715ea672cb 21 Serial gps(PB_4,PB_3, 9600);//pin19,20 tx,rx
Zero0000 0:aa715ea672cb 22 DigitalOut key(PA_8);//pin22
Zero0000 0:aa715ea672cb 23 Serial pc(PA_10,PA_9, 115200);//pin8,9 TX,RX
Zero0000 0:aa715ea672cb 24 BME280 bme = BME280(PB_7, PB_6);//pin30,29 SDA,SCL
Zero0000 0:aa715ea672cb 25 SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
Zero0000 0:aa715ea672cb 26 Timer timer_standby;
Zero0000 0:aa715ea672cb 27 Timer timer_flight;
Zero0000 0:aa715ea672cb 28 Timer timer_burning;
Zero0000 0:aa715ea672cb 29 FILE *fp;
Zero0000 0:aa715ea672cb 30 FILE *al;
Zero0000 0:aa715ea672cb 31 FILE *ac;
Zero0000 0:aa715ea672cb 32 //関数
Zero0000 0:aa715ea672cb 33 float _getAlt();
Zero0000 0:aa715ea672cb 34 float _median(float data[],int num);
Zero0000 0:aa715ea672cb 35 void _SendGPS();
Zero0000 0:aa715ea672cb 36 float _DMS2DEG(float raw_data);
Zero0000 0:aa715ea672cb 37 void _calcurateAcc();
Zero0000 0:aa715ea672cb 38
Zero0000 0:aa715ea672cb 39
Zero0000 0:aa715ea672cb 40 void mpu_init(){
Zero0000 0:aa715ea672cb 41 uint8_t whoami_MPU9250, whoami_AK8963;
Zero0000 0:aa715ea672cb 42 float mag_init[3];
Zero0000 0:aa715ea672cb 43
Zero0000 0:aa715ea672cb 44 whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
Zero0000 0:aa715ea672cb 45 whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
Zero0000 0:aa715ea672cb 46 pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
Zero0000 0:aa715ea672cb 47 pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
Zero0000 0:aa715ea672cb 48
Zero0000 0:aa715ea672cb 49 if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){
Zero0000 0:aa715ea672cb 50 pc.printf("MPU9250 is detected.\n\r");
Zero0000 0:aa715ea672cb 51 wait(0.1);
Zero0000 0:aa715ea672cb 52 mpu.resetMPU9250();
Zero0000 0:aa715ea672cb 53 mpu.initMPU9250();
Zero0000 0:aa715ea672cb 54 wait(0.1);
Zero0000 0:aa715ea672cb 55 mpu.initAK8963(mag_init);
Zero0000 0:aa715ea672cb 56 mpu.getGres();
Zero0000 0:aa715ea672cb 57 mpu.getAres();
Zero0000 0:aa715ea672cb 58 mpu.getMres();
Zero0000 0:aa715ea672cb 59 wait(0.1);
Zero0000 0:aa715ea672cb 60 }
Zero0000 0:aa715ea672cb 61 else{
Zero0000 0:aa715ea672cb 62 pc.printf("Could not detect MPU9250.\n\r");
Zero0000 0:aa715ea672cb 63 pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
Zero0000 0:aa715ea672cb 64 whoami_MPU9250, whoami_AK8963);
Zero0000 0:aa715ea672cb 65 while(1);
Zero0000 0:aa715ea672cb 66 }
Zero0000 0:aa715ea672cb 67 }
Zero0000 0:aa715ea672cb 68
Zero0000 0:aa715ea672cb 69 enum PHASE{Standby=0,Flight=1,Burning=3,Parachute=7} Phase;
Zero0000 0:aa715ea672cb 70 float alt_gnd;
Zero0000 0:aa715ea672cb 71 float alt_max;
Zero0000 0:aa715ea672cb 72 char c[516];
Zero0000 0:aa715ea672cb 73 int i = 0;
Zero0000 0:aa715ea672cb 74 int cnt_gps=0;
Zero0000 0:aa715ea672cb 75 int timeout;
Zero0000 0:aa715ea672cb 76 int cnt_data;
Zero0000 0:aa715ea672cb 77 int cnt_judge=0;
Zero0000 0:aa715ea672cb 78 int16_t acc[3];
Zero0000 0:aa715ea672cb 79
Zero0000 0:aa715ea672cb 80 float buff_ax[N], buff_ay[N], buff_az[N];
Zero0000 0:aa715ea672cb 81 float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
Zero0000 0:aa715ea672cb 82 float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
Zero0000 0:aa715ea672cb 83 float ax = 0.0f, ay = 0.0f, az = 0.0f;
Zero0000 0:aa715ea672cb 84 int cnt = 0;
Zero0000 0:aa715ea672cb 85 float acc_abs;
Zero0000 0:aa715ea672cb 86 float alt_buff[10],alt_md;
Zero0000 0:aa715ea672cb 87
Zero0000 0:aa715ea672cb 88 int main(){
Zero0000 0:aa715ea672cb 89
Zero0000 0:aa715ea672cb 90 wait(0.1);
Zero0000 0:aa715ea672cb 91 mpu_init();
Zero0000 0:aa715ea672cb 92 bme.initialize();//BME初期化
Zero0000 0:aa715ea672cb 93 mkdir("/sd/mydir",0777);//SDファイル作成
Zero0000 0:aa715ea672cb 94 fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外デ
Zero0000 0:aa715ea672cb 95 al = fopen("/sd/mydir/altitude.txt", "a");
Zero0000 0:aa715ea672cb 96 ac = fopen("/sd/mydir/acc.txt", "a");
Zero0000 0:aa715ea672cb 97 if(fp == NULL) {
Zero0000 0:aa715ea672cb 98 error("Could not open file for write\n");
Zero0000 0:aa715ea672cb 99 }
Zero0000 0:aa715ea672cb 100
Zero0000 0:aa715ea672cb 101 wait(0.1);
Zero0000 0:aa715ea672cb 102
Zero0000 0:aa715ea672cb 103
Zero0000 0:aa715ea672cb 104 switch(Phase){
Zero0000 0:aa715ea672cb 105 case Standby: //待機モード
Zero0000 0:aa715ea672cb 106 pc.printf("Standby\n\r");
Zero0000 0:aa715ea672cb 107 key = 0;
Zero0000 0:aa715ea672cb 108 if(pc.readable()){
Zero0000 0:aa715ea672cb 109 c[0]=pc.getc();
Zero0000 0:aa715ea672cb 110 }
Zero0000 0:aa715ea672cb 111
Zero0000 0:aa715ea672cb 112 if(c[0] == 'f'){
Zero0000 0:aa715ea672cb 113 pc.printf("Flight\r\n");
Zero0000 0:aa715ea672cb 114 Phase = Flight;
Zero0000 0:aa715ea672cb 115 timer_standby.stop();
Zero0000 0:aa715ea672cb 116 timer_flight.start();
Zero0000 0:aa715ea672cb 117 }
Zero0000 0:aa715ea672cb 118
Zero0000 0:aa715ea672cb 119 break;
Zero0000 0:aa715ea672cb 120
Zero0000 0:aa715ea672cb 121 case Flight:
Zero0000 0:aa715ea672cb 122 key = 0;
Zero0000 0:aa715ea672cb 123 while(1){
Zero0000 0:aa715ea672cb 124 mpu.readAccelData(acc);
Zero0000 0:aa715ea672cb 125 _calcurateAcc();
Zero0000 0:aa715ea672cb 126 acc_abs = sqrt(pow(ax/9.81f,2)+pow(ay/9.81f,2)+pow(az/9.81f,2));//合成加速度
Zero0000 0:aa715ea672cb 127 fp = fopen("/sd/mydir/gps.txt", "a");
Zero0000 0:aa715ea672cb 128 _SendGPS();
Zero0000 0:aa715ea672cb 129 fclose(fp);
Zero0000 0:aa715ea672cb 130 timeout = timer_flight.read();
Zero0000 0:aa715ea672cb 131 ac = fopen("/sd/mydir/acc.txt","a");
Zero0000 0:aa715ea672cb 132 fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 133 fclose(ac);
Zero0000 0:aa715ea672cb 134 pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 135 /*加速度判定*/
Zero0000 0:aa715ea672cb 136 if(acc_abs>ACC_JUDGE_LAUNCH){
Zero0000 0:aa715ea672cb 137 cnt_judge++;
Zero0000 0:aa715ea672cb 138 }
Zero0000 0:aa715ea672cb 139 if(cnt_judge==CNT_LAUNCH){
Zero0000 0:aa715ea672cb 140 cnt_judge=0;
Zero0000 0:aa715ea672cb 141 timer_burning.start();
Zero0000 0:aa715ea672cb 142 Phase = Burning;
Zero0000 0:aa715ea672cb 143 timer_flight.stop();
Zero0000 0:aa715ea672cb 144 }
Zero0000 0:aa715ea672cb 145 }
Zero0000 0:aa715ea672cb 146 break;
Zero0000 0:aa715ea672cb 147 case Burning:
Zero0000 0:aa715ea672cb 148 key = 0;
Zero0000 0:aa715ea672cb 149 while(timer_flight.read() < TIME_BURNING){
Zero0000 0:aa715ea672cb 150 mpu.readAccelData(acc);
Zero0000 0:aa715ea672cb 151 _calcurateAcc();
Zero0000 0:aa715ea672cb 152 pc.printf("Burning\n\r");
Zero0000 0:aa715ea672cb 153 fp = fopen("/sd/mydir/gps.txt", "a");
Zero0000 0:aa715ea672cb 154 _SendGPS();
Zero0000 0:aa715ea672cb 155 fclose(fp);
Zero0000 0:aa715ea672cb 156 timeout = timer_flight.read();
Zero0000 0:aa715ea672cb 157 ac = fopen("/sd/mydir/acc.txt","a");
Zero0000 0:aa715ea672cb 158 fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 159 fclose(ac);
Zero0000 0:aa715ea672cb 160 pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 161
Zero0000 0:aa715ea672cb 162 }
Zero0000 0:aa715ea672cb 163 Phase = Parachute;
Zero0000 0:aa715ea672cb 164 pc.printf("Parachute\n\r");
Zero0000 0:aa715ea672cb 165 break;
Zero0000 0:aa715ea672cb 166
Zero0000 0:aa715ea672cb 167
Zero0000 0:aa715ea672cb 168 case Parachute:
Zero0000 0:aa715ea672cb 169 while(1){
Zero0000 0:aa715ea672cb 170 mpu.readAccelData(acc);
Zero0000 0:aa715ea672cb 171 _calcurateAcc();
Zero0000 0:aa715ea672cb 172 fp = fopen("/sd/mydir/gps.txt", "a");
Zero0000 0:aa715ea672cb 173 _SendGPS();
Zero0000 0:aa715ea672cb 174 fclose(fp);
Zero0000 0:aa715ea672cb 175 timeout = timer_flight.read();
Zero0000 0:aa715ea672cb 176 ac = fopen("/sd/mydir/acc.txt","a");
Zero0000 0:aa715ea672cb 177 fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 178 fclose(ac);
Zero0000 0:aa715ea672cb 179 pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
Zero0000 0:aa715ea672cb 180
Zero0000 0:aa715ea672cb 181 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Zero0000 0:aa715ea672cb 182 alt_buff[cnt_data] = _getAlt();
Zero0000 0:aa715ea672cb 183 }
Zero0000 0:aa715ea672cb 184 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
Zero0000 0:aa715ea672cb 185 alt_md = alt_md - alt_gnd;
Zero0000 0:aa715ea672cb 186
Zero0000 0:aa715ea672cb 187 if(alt_md > alt_max){
Zero0000 0:aa715ea672cb 188 alt_max = alt_md;
Zero0000 0:aa715ea672cb 189 cnt_judge = 0;
Zero0000 0:aa715ea672cb 190 }
Zero0000 0:aa715ea672cb 191 else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
Zero0000 0:aa715ea672cb 192 cnt_judge++;
Zero0000 0:aa715ea672cb 193 }
Zero0000 0:aa715ea672cb 194 //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
Zero0000 0:aa715ea672cb 195 if(cnt_judge == CNT_JUDGE){
Zero0000 0:aa715ea672cb 196 key = 1;
Zero0000 0:aa715ea672cb 197 pc.printf("Open\n\r");
Zero0000 0:aa715ea672cb 198 }
Zero0000 0:aa715ea672cb 199 break;
Zero0000 0:aa715ea672cb 200
Zero0000 0:aa715ea672cb 201 }
Zero0000 0:aa715ea672cb 202 }
Zero0000 0:aa715ea672cb 203 }
Zero0000 0:aa715ea672cb 204
Zero0000 0:aa715ea672cb 205
Zero0000 0:aa715ea672cb 206
Zero0000 0:aa715ea672cb 207
Zero0000 0:aa715ea672cb 208
Zero0000 0:aa715ea672cb 209 float _getAlt(){
Zero0000 0:aa715ea672cb 210 float altitude,pressure,temperature;
Zero0000 0:aa715ea672cb 211 temperature = bme.getTemperature();
Zero0000 0:aa715ea672cb 212 pressure = bme.getPressure();
Zero0000 0:aa715ea672cb 213 altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
Zero0000 0:aa715ea672cb 214 return altitude;
Zero0000 0:aa715ea672cb 215 }
Zero0000 0:aa715ea672cb 216
Zero0000 0:aa715ea672cb 217 float _DMS2DEG(float raw_data){
Zero0000 0:aa715ea672cb 218 int d=(int)(raw_data/100);
Zero0000 0:aa715ea672cb 219 float m=(raw_data-(float)d*100);
Zero0000 0:aa715ea672cb 220 return (float)d+m/60;
Zero0000 0:aa715ea672cb 221 }
Zero0000 0:aa715ea672cb 222 float _median(float data[], int num){
Zero0000 0:aa715ea672cb 223 float *data_cpy, ans;
Zero0000 0:aa715ea672cb 224 data_cpy = new float[num];
Zero0000 0:aa715ea672cb 225 memcpy(data_cpy,data,sizeof(float)*num);
Zero0000 0:aa715ea672cb 226 for(i=0; i<num; i++){
Zero0000 0:aa715ea672cb 227 for(int j=0; j<num-i-1; j++){
Zero0000 0:aa715ea672cb 228 if(data_cpy[j]>data_cpy[j+1]){
Zero0000 0:aa715ea672cb 229 float buff = data_cpy[j+1];
Zero0000 0:aa715ea672cb 230 data_cpy[j+1] = data_cpy[j];
Zero0000 0:aa715ea672cb 231 data_cpy[j] = buff;
Zero0000 0:aa715ea672cb 232 }
Zero0000 0:aa715ea672cb 233 }
Zero0000 0:aa715ea672cb 234 }
Zero0000 0:aa715ea672cb 235
Zero0000 0:aa715ea672cb 236 if(num%2!=0) ans = data_cpy[num/2];
Zero0000 0:aa715ea672cb 237 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
Zero0000 0:aa715ea672cb 238 delete[] data_cpy;
Zero0000 0:aa715ea672cb 239 return ans;
Zero0000 0:aa715ea672cb 240 }
Zero0000 0:aa715ea672cb 241
Zero0000 0:aa715ea672cb 242
Zero0000 0:aa715ea672cb 243 void _SendGPS(){
Zero0000 0:aa715ea672cb 244 char gps_data[256];
Zero0000 0:aa715ea672cb 245 while(1){
Zero0000 0:aa715ea672cb 246 if(gps.readable()){
Zero0000 0:aa715ea672cb 247 gps_data[cnt_gps] = gps.getc();
Zero0000 0:aa715ea672cb 248 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
Zero0000 0:aa715ea672cb 249 cnt_gps = 0;
Zero0000 0:aa715ea672cb 250 memset(gps_data,'\0',256);
Zero0000 0:aa715ea672cb 251 }else if(gps_data[cnt_gps] == '\r'){
Zero0000 0:aa715ea672cb 252 float world_time, lon_east, lat_north;
Zero0000 0:aa715ea672cb 253 int rlock, sat_num;
Zero0000 0:aa715ea672cb 254 char lat,lon;
Zero0000 0:aa715ea672cb 255 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
Zero0000 0:aa715ea672cb 256 if(rlock==1){
Zero0000 0:aa715ea672cb 257 lat_north = _DMS2DEG(lat_north);
Zero0000 0:aa715ea672cb 258 lon_east = _DMS2DEG(lon_east);
Zero0000 0:aa715ea672cb 259 pc.printf("%s\r\n",gps_data);
Zero0000 0:aa715ea672cb 260 pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
Zero0000 0:aa715ea672cb 261 pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
Zero0000 0:aa715ea672cb 262 for(i=0;i<2;i++){
Zero0000 0:aa715ea672cb 263 c[i]=pc.getc();
Zero0000 0:aa715ea672cb 264 }
Zero0000 0:aa715ea672cb 265 break;
Zero0000 0:aa715ea672cb 266 }else{
Zero0000 0:aa715ea672cb 267 //pc.printf("%s\r\n",gps_data);
Zero0000 0:aa715ea672cb 268 pc.printf("NoGPSSignal\r\n");
Zero0000 0:aa715ea672cb 269 break;
Zero0000 0:aa715ea672cb 270 }
Zero0000 0:aa715ea672cb 271 }else{
Zero0000 0:aa715ea672cb 272 //pc.printf("No_Satellite_signal\r\n");
Zero0000 0:aa715ea672cb 273 }
Zero0000 0:aa715ea672cb 274 }else{
Zero0000 0:aa715ea672cb 275 cnt_gps++;
Zero0000 0:aa715ea672cb 276 }
Zero0000 0:aa715ea672cb 277 }
Zero0000 0:aa715ea672cb 278 }
Zero0000 0:aa715ea672cb 279 }
Zero0000 0:aa715ea672cb 280
Zero0000 0:aa715ea672cb 281 void _calcurateAcc(){
Zero0000 0:aa715ea672cb 282 /* 5回移動平均 */
Zero0000 0:aa715ea672cb 283 sum_ax = sum_ax - buff_ax[cnt];
Zero0000 0:aa715ea672cb 284 sum_ay = sum_ay - buff_ay[cnt];
Zero0000 0:aa715ea672cb 285 sum_az = sum_az - buff_az[cnt];
Zero0000 0:aa715ea672cb 286
Zero0000 0:aa715ea672cb 287 ax_new = acc[0] / 2049.81;
Zero0000 0:aa715ea672cb 288 ay_new = acc[1] / 2049.81;
Zero0000 0:aa715ea672cb 289 az_new = acc[2] / 2049.81;
Zero0000 0:aa715ea672cb 290
Zero0000 0:aa715ea672cb 291 buff_ax[cnt] = ax_new;
Zero0000 0:aa715ea672cb 292 buff_ay[cnt] = ay_new;
Zero0000 0:aa715ea672cb 293 buff_az[cnt] = az_new;
Zero0000 0:aa715ea672cb 294
Zero0000 0:aa715ea672cb 295 sum_ax = sum_ax + buff_ax[cnt];
Zero0000 0:aa715ea672cb 296 sum_ay = sum_ay + buff_ay[cnt];
Zero0000 0:aa715ea672cb 297 sum_az = sum_az + buff_az[cnt];
Zero0000 0:aa715ea672cb 298
Zero0000 0:aa715ea672cb 299 cnt++;
Zero0000 0:aa715ea672cb 300 if(cnt == N) cnt = 0;
Zero0000 0:aa715ea672cb 301
Zero0000 0:aa715ea672cb 302 ax = sum_ax / N;
Zero0000 0:aa715ea672cb 303 ay = sum_ay / N;
Zero0000 0:aa715ea672cb 304 az = sum_az / N;
Zero0000 0:aa715ea672cb 305 }