CORE
能代宇宙イベント2020,ロケット用プログラム Nucleo-F303K8 ,MPU9250,BME280
Dependencies: mbed SDFileSystem BME280 MPU9250
Diff: main.cpp
- Revision:
- 0:aa715ea672cb
diff -r 000000000000 -r aa715ea672cb main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,305 @@
+#include "mbed.h"
+#include "math.h"
+#include "MPU9250.h"
+#include "BME280.h"
+#include "SDFileSystem.h"
+#define NUM_CNT_MEDIAN 10 //中央値をとる個数
+#define CNT_LAUNCH_TIMES 5//発射判定に必要な連続数
+#define TIME_SEND 1.0 //無線送信する間隔
+#define CNT_LAUNCH 5 //発射判定するときのしきい値(TBD)
+#define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値
+#define TIME_BURNING 6 //燃焼時間(TBD)
+#define ALT_JUDGE_OPEN 5 //落下判定のカウントを１増やす高度差
+#define CNT_JUDGE 10 //頂点判定する時に落下のカウント数
+#define p0 1013.25f //海面気圧
+#define N 5
+#define sampleFreq 100.0f
+#define beta 0.33f // gain（大きいと加速度による補正が早い）
+#define PI 3.14159265358979323846f
+
+MPU9250 mpu = MPU9250(PB_7,PB_6);// pin30,29 SDA,SCL
+Serial gps(PB_4,PB_3, 9600);//pin19,20 tx,rx
+DigitalOut key(PA_8);//pin22
+Serial pc(PA_10,PA_9, 115200);//pin8,9 TX,RX
+BME280 bme = BME280(PB_7, PB_6);//pin30,29 SDA,SCL
+SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
+Timer timer_standby;
+Timer timer_flight;
+Timer timer_burning;
+FILE *fp;
+FILE *al;
+FILE *ac;
+//関数
+float _getAlt();
+float _median(float data[],int num);
+void _SendGPS();
+float _DMS2DEG(float raw_data);
+void _calcurateAcc();
+
+
+void mpu_init(){
+ uint8_t whoami_MPU9250, whoami_AK8963;
+ float mag_init[3];
+
+ whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+ whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
+ pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
+ pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
+
+ if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){
+ pc.printf("MPU9250 is detected.\n\r");
+ wait(0.1);
+ mpu.resetMPU9250();
+ mpu.initMPU9250();
+ wait(0.1);
+ mpu.initAK8963(mag_init);
+ mpu.getGres();
+ mpu.getAres();
+ mpu.getMres();
+ wait(0.1);
+ }
+ else{
+ pc.printf("Could not detect MPU9250.\n\r");
+ pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
+ whoami_MPU9250, whoami_AK8963);
+ while(1);
+ }
+}
+
+enum PHASE{Standby=0,Flight=1,Burning=3,Parachute=7} Phase;
+float alt_gnd;
+float alt_max;
+char c[516];
+int i = 0;
+int cnt_gps=0;
+int timeout;
+int cnt_data;
+int cnt_judge=0;
+int16_t acc[3];
+
+float buff_ax[N], buff_ay[N], buff_az[N];
+float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
+float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
+float ax = 0.0f, ay = 0.0f, az = 0.0f;
+int cnt = 0;
+float acc_abs;
+float alt_buff[10],alt_md;
+
+int main(){
+
+ wait(0.1);
+ mpu_init();
+ bme.initialize();//BME初期化
+ mkdir("/sd/mydir",0777);//SDファイル作成
+ fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外デ
+ al = fopen("/sd/mydir/altitude.txt", "a");
+ ac = fopen("/sd/mydir/acc.txt", "a");
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ }
+
+ wait(0.1);
+
+
+ switch(Phase){
+ case Standby: //待機モード
+ pc.printf("Standby\n\r");
+ key = 0;
+ if(pc.readable()){
+ c[0]=pc.getc();
+ }
+
+ if(c[0] == 'f'){
+ pc.printf("Flight\r\n");
+ Phase = Flight;
+ timer_standby.stop();
+ timer_flight.start();
+ }
+
+ break;
+
+ case Flight:
+ key = 0;
+ while(1){
+ mpu.readAccelData(acc);
+ _calcurateAcc();
+ acc_abs = sqrt(pow(ax/9.81f,2)+pow(ay/9.81f,2)+pow(az/9.81f,2));//合成加速度
+ fp = fopen("/sd/mydir/gps.txt", "a");
+ _SendGPS();
+ fclose(fp);
+ timeout = timer_flight.read();
+ ac = fopen("/sd/mydir/acc.txt","a");
+ fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+ fclose(ac);
+ pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+ /*加速度判定*/
+ if(acc_abs>ACC_JUDGE_LAUNCH){
+ cnt_judge++;
+ }
+ if(cnt_judge==CNT_LAUNCH){
+ cnt_judge=0;
+ timer_burning.start();
+ Phase = Burning;
+ timer_flight.stop();
+ }
+ }
+ break;
+ case Burning:
+ key = 0;
+ while(timer_flight.read() < TIME_BURNING){
+ mpu.readAccelData(acc);
+ _calcurateAcc();
+ pc.printf("Burning\n\r");
+ fp = fopen("/sd/mydir/gps.txt", "a");
+ _SendGPS();
+ fclose(fp);
+ timeout = timer_flight.read();
+ ac = fopen("/sd/mydir/acc.txt","a");
+ fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+ fclose(ac);
+ pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+
+ }
+ Phase = Parachute;
+ pc.printf("Parachute\n\r");
+ break;
+
+
+ case Parachute:
+ while(1){
+ mpu.readAccelData(acc);
+ _calcurateAcc();
+ fp = fopen("/sd/mydir/gps.txt", "a");
+ _SendGPS();
+ fclose(fp);
+ timeout = timer_flight.read();
+ ac = fopen("/sd/mydir/acc.txt","a");
+ fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+ fclose(ac);
+ pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+
+ for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
+ alt_buff[cnt_data] = _getAlt();
+ }
+ alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
+ alt_md = alt_md - alt_gnd;
+
+ if(alt_md > alt_max){
+ alt_max = alt_md;
+ cnt_judge = 0;
+ }
+ else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
+ cnt_judge++;
+ }
+ //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
+ if(cnt_judge == CNT_JUDGE){
+ key = 1;
+ pc.printf("Open\n\r");
+ }
+ break;
+
+ }
+ }
+ }
+
+
+
+
+
+float _getAlt(){
+ float altitude,pressure,temperature;
+ temperature = bme.getTemperature();
+ pressure = bme.getPressure();
+ altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
+ return altitude;
+}
+
+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(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;
+}
+
+
+void _SendGPS(){
+ char gps_data[256];
+ while(1){
+ 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);
+ pc.printf("%s\r\n",gps_data);
+ pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+ pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
+ for(i=0;i<2;i++){
+ c[i]=pc.getc();
+ }
+ break;
+ }else{
+ //pc.printf("%s\r\n",gps_data);
+ pc.printf("NoGPSSignal\r\n");
+ break;
+ }
+ }else{
+ //pc.printf("No_Satellite_signal\r\n");
+ }
+ }else{
+ cnt_gps++;
+ }
+ }
+ }
+}
+
+void _calcurateAcc(){
+ /* 5回移動平均 */
+ sum_ax = sum_ax - buff_ax[cnt];
+ sum_ay = sum_ay - buff_ay[cnt];
+ sum_az = sum_az - buff_az[cnt];
+
+ ax_new = acc[0] / 2049.81;
+ ay_new = acc[1] / 2049.81;
+ az_new = acc[2] / 2049.81;
+
+ buff_ax[cnt] = ax_new;
+ buff_ay[cnt] = ay_new;
+ buff_az[cnt] = az_new;
+
+ sum_ax = sum_ax + buff_ax[cnt];
+ sum_ay = sum_ay + buff_ay[cnt];
+ sum_az = sum_az + buff_az[cnt];
+
+ cnt++;
+ if(cnt == N) cnt = 0;
+
+ ax = sum_ax / N;
+ ay = sum_ay / N;
+ az = sum_az / N;
+ }
\ No newline at end of file