Haruki Sashida
201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 4:4b3ae90ec778
- Parent:
- 3:24a8901befb6
- Child:
- 5:f6e956e8a060
--- a/main.cpp Thu Nov 02 17:22:14 2017 +0000
+++ b/main.cpp Thu Aug 09 21:39:18 2018 +0000
@@ -1,47 +1,330 @@
-/*
-説明
-Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム
-
-ライブラリ
-https://developer.mbed.org/users/spiridion/code/BMP180/
-
-以下ピン配置
-Nucleo BMP180
-GND-----GND-------0V
-+3V3----VIN
-D4------SDA
-D5------SCL
-
-*/
#include "mbed.h"
#include "math.h"
+#include "MPU6050.h"
#include "BMP180.h"
-#define p0 1013.25f//海面気圧
+#include "SDFileSystem.h"
+
+/*マクロ処理*/
+//動作レート
+#define RATE_LOG 20
+#define RATE_OPEN 50
+//発射判定
+#define ACC_JUDGE_LAUNCH 1.5
+#define CNT_JUDGE_LAUNCH 5
+//待機時間
+#define TIME_GAP 3.0
+//頂点判定
+#define ALT_JUDGE_OPEN 1.5
+#define CNT_JUDGE_OPEN 5
+#define TIME_JUDGE_OPEN 30
+//放出判定
+#define TIME_JUDGE_OUT 60
+#define ALT_JUDGE_OUT -1.5
+#define CNT_JUDGE_OUT 5
+//共通
+#define TIME_JUDGE_CNT 1.5
+#define p0 1013.25f
+//ログ //2018.06.28 edit
+#define NUM_DATA 20
-DigitalOut myled(LED1);
-Serial pc(USBTX,USBRX);
-BMP180 bmp(p28, p27);
-Timer timer;
+MPU6050 mpu(p9,p10);
+BMP180 bmp(p9,p10);
+BusOut led(LED1,LED2,LED3,LED4);
+Serial pc(USBTX,USBRX);
+Serial twe(p28,p27);
+Serial gps(p13,p14);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+PwmOut servo3(p23);
+PwmOut servo4(p24);
+DigitalOut sw(p25);
+SDFileSystem sd(p5,p6,p7,p8,"sd");
+LocalFileSystem local("local");
+
+Timer timer_open;
+Timer timer_log;
+Ticker tic_open;
+Ticker tic_log;
-float getAlt(float press, float temp);
+void _open();
+void _log();
+float _getAlt();
+float _DMS2DEG(float raw_data);
+float _median(float data[],int num);
+int _input(char cha);
+
+float Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること
+float Time_alt;
+float Data[2][NUM_DATA][8];
+int Cnt=0, Cnt_acc=0, Cnt_alt=0;
+int Cnt_data=0;
+int cnt_gps;
+char gps_data[256];
+bool Row;
+FILE *lfp;
+FILE *fp;
+
+enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase;
+enum SERVO{LOCK=0,UNLOCK=1} Servo;
-int main() {
- float pressure,temperature,altitude;
- float time;
+int main(){
+ /*センサ類初期化*/
bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
- pc.printf("time, temperature ,pressure, altitude\r\n");
- timer.start();
-
- while(1) {
- bmp.ReadData(&temperature,&pressure);
- altitude = getAlt(pressure,temperature);
- time = timer.read();
- pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude);
- myled =! myled;
- wait(1);
+ twe.baud(115200);
+ sw=1;
+ /*ファイル作成*/
+ mkdir("/sd/mydir",0777);
+ fp=fopen("/sd/mydir/data.txt","a");
+ fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
+ fclose(fp);
+ lfp=fopen("/local/data.txt","a");
+ fclose(lfp);
+ /*サーボ調整*/
+ servo1.period_ms(20);
+ servo2.period_ms(20);
+ servo3.period_ms(20);
+ servo4.period_ms(20);
+ /*初めの挨拶*/
+ //pc.printf("POWER ON\r\n");
+ twe.printf("POWER ON\r\n");
+ /*入力待ち*/
+ while(1){
+ char cha = twe.getc();
+ if(_input(cha)==-1){
+ timer_log.start();
+ tic_log.attach(&_log, 1.0/RATE_LOG);
+ break;
+ }
+ }
+ /*フライト準備*/
+ Phase = SETUP;
+ tic_open.attach(&_open, 1.0/RATE_OPEN);
+
+ /*GPS*/
+ while(1){
+ /*GPS取得*/
+ if(Phase != RISE){
+ 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++;
+ }
+ }
+ }
+
+ if(timer_log.read()>=30.0*60.0){
+ timer_log.reset();
+ /*なんかずっとこればっか書き込まれた 2018/7/1
+ fp=fopen("/sd/mydir/data.txt","w");
+ fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
+ fclose(fp);
+ */
+ }
}
}
-float getAlt(float press, float temp){
- return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
-}
\ No newline at end of file
+void _open(){
+ led = Phase;
+ float acc[3],acc_buf[10],acc_abs,time_acc,time_alt;
+ switch(Phase){
+ case SETUP: //地上高度取得
+ //pc.printf("Hello,World!\r\n");
+ twe.printf("Hello,World!\r\n");
+ for(Cnt=0;Cnt<10;Cnt++){
+ Alt_buf[Cnt] = _getAlt();
+ }
+ Alt_gnd = _median(Alt_buf,10);
+ /*データをローカルファイルに保存*/
+ lfp=fopen("/local/data.txt", "a");
+ fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
+ fclose(lfp);
+
+ timer_open.start();
+ Phase = LAUNCH;
+ //pc.printf("Phase = LAUNCH\r\n");
+ twe.printf("Phase = LAUNCH\r\n");
+ break;
+
+ case LAUNCH: //発射判定
+ for(Cnt=0;Cnt<10;Cnt++){
+ mpu.getAccelero(acc);
+ acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2));
+ }
+ acc_abs = _median(acc_buf,10);
+ //pc.printf("acc:%f\r\n",acc_abs);
+ //pc.printf("cnt:%d\r\n",Cnt_acc);
+ //加速度判定
+ if(acc_abs>ACC_JUDGE_LAUNCH){
+ Cnt_acc++;
+ time_acc = timer_open.read();
+ }
+ if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0;
+ if(Cnt_acc == CNT_JUDGE_LAUNCH){
+ //pc.printf("Phase = WAIT\r\n");
+ twe.printf("LAUNCHED\r\n");
+ timer_open.reset();
+ Phase = WAIT;
+ }
+ break;
+
+ case WAIT: //待機時間
+ if(timer_open.read()>TIME_GAP) Phase = RISE;
+ break;
+
+ case RISE: //頂点判定
+ for(Cnt=0;Cnt<5;Cnt++){
+ Alt_buf[Cnt] = _getAlt();
+ //pc.printf("alt:%f\r\n",Alt_buf[Cnt]);
+ }
+ Alt_md = _median(Alt_buf,5);
+ twe.printf("alt:%f\r\n",Alt_md);
+ //twe.printf("cnt:%d\r\n",Cnt_alt);
+ if(Alt_md > Alt_max){
+ Alt_max = Alt_md;
+ Cnt_alt = 0;
+ }
+ else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){
+ Cnt_alt++;
+ Time_alt = timer_open.read();
+ }
+ if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){
+ Cnt_alt = 0;
+ }
+ if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){
+ //pc.printf("Phase = DROP1\r\n");
+ twe.printf("OPEN PARA\r\n");
+ Cnt_alt = 0;
+ /*ローカルファイルに最高高度を記録*/
+ lfp = fopen("/local/data.txt", "a");
+ fprintf(lfp,"最高高度：%f\r\n",Alt_max);
+ fclose(lfp);
+
+ servo1.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
+ servo2.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
+ Phase = DROP1;
+ }
+ break;
+
+ case DROP1: //放出機構作動？
+ for(Cnt=0;Cnt<5;Cnt++){
+ Alt_buf[Cnt] = _getAlt();
+ }
+ Alt_md = _median(Alt_buf,5);
+ twe.printf("Alt:%f\r\n",Alt_md);
+ //twe.printf("Cnt:%d\r\n",Cnt_alt);
+ if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){
+ Cnt_alt++;
+ }
+ if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする？到達高度低いかも？
+ //pc.printf("Phase = DROP2\r\n");
+ twe.printf("GO\r\n");
+ servo3.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
+ servo4.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
+ Phase = DROP2;
+ }
+ break;
+
+ case DROP2: //着水検知．サーボの電源落とす．
+ //twe.printf("cnt:%d\r\n",Cnt_alt);
+ wait(3.0);
+ sw = 0;
+ //pc.printf("Goodbye Servo\r\n");
+ twe.printf("FINISH\r\n");
+ Phase=SEA;
+ break;
+ }
+}
+
+void _log(){
+ Data[Row][Cnt_data][0]=Phase;
+ Data[Row][Cnt_data][1]=timer_log.read();
+ mpu.getAccelero(&Data[Row][Cnt_data][2]);
+ bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]);
+ Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f;
+ Cnt_data++;
+ //pc.printf("Cnt_data:%d\r\n",Cnt_data);
+ if(Cnt_data==NUM_DATA){
+ Cnt_data = 0;
+ Row =! Row;
+ fp = fopen("/sd/mydir/data.txt","a");
+ for(int i=0;i<NUM_DATA;i++){
+ fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]);
+ }
+ fclose(fp);
+ }
+}
+
+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;
+}
+
+int _input(char cha){
+ twe.printf("%c\r\n",cha);
+ if(cha!='\0'){
+// pc.printf("%c\r\n",c);
+ }
+ if(cha=='F'){
+ twe.printf("flight mode on\r\n");
+ return -1;
+ }else if(cha=='u'){ //開放機構のUNLOCK
+ servo1.pulsewidth(0.0006);
+ servo2.pulsewidth(0.0006);
+ }else if(cha=='l'){ //開放機構のLOCK
+ servo1.pulsewidth(0.0024);
+ servo2.pulsewidth(0.0024);
+ }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU)
+ servo3.pulsewidth(0.0010);
+ servo4.pulsewidth(0.0010);
+ }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU)
+ servo3.pulsewidth(0.0024);
+ servo4.pulsewidth(0.0024);
+ }
+ return 0;
+}
+
+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;
+}
\ No newline at end of file