CORE
2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 14:cbee1bfdfca7
- Parent:
- 13:f73b4beded57
- Child:
- 15:01d3969b89d0
--- a/main.cpp Fri Mar 03 05:14:02 2017 +0000
+++ b/main.cpp Wed Mar 08 15:50:06 2017 +0000
@@ -43,48 +43,45 @@
#include "MS5607I2C.h"
#include "MPU6050.h"
#include "SDFileSystem.h"
-/*判定パラメータ*/
-#define ACC_LAUNCH 4//発射判定加速度[g]
-#define DROP_TOP 1.5//頂点判定最低降下量[m]
-#define ALT_CAN 150//缶サット開放高度[m]
-#define TIME_TOP 15//タイマー開放時間[s]
-/*動作レート*/
-#define RATE_LOG 10//ログ用
-#define RATE_OPEN 10//開放用
+
+#define ACC_LAUNCH 4
+#define TOP_DROP_AMOUNT 1.5
+#define ALT_UAV_DROP 150
+#define TIME_REACH_TOP 15
+
+#define RATE_LOG 10
+#define RATE_OPEN 10
/*サーボ動作*/
-#define LOCK 0.0005
-#define UNLOCK 0.0015
-/*フェーズ定義*/
-#define STANDBY 0
-#define LAUNCH 1
-#define FLIGHT 2
-#define DROP 3
-/*入力モード定義*/
-#define BAN 0
-#define NORMAL 1
-#define SERVO_1 2
-#define SERVO_2 3
-#define MODE 4
-#define DEBUG 5
+#define LOCK 0.0005
+#define UNLOCK 0.0015
+/*モード定義*/
+#define STANDBY 0
+#define TEST 1
+#define FLIGHT 2
+/*開放フェーズ定義*/
+#define SETUP 0
+#define LAUNCH 1
+#define RISE 2
+#define DROP 3
/**/
-#define P0 1013.25f//海面気圧[hPa]
-#define ACC 4096.0f//加速度オフセット値
+#define P0 1013.25f//海面気圧[hPa]
+#define ACC 4096.0f//加速度オフセット値
/*ピン指定*/
-MS5607I2C ms5607(p9, p10, false);
-MPU6050 mpu(p9,p10);
-BusOut myled(LED1,LED2,LED3,LED4);
-SDFileSystem sd(p11, p12, p13, p14, "sd");
-Serial device(USBTX, USBRX);
-PwmOut Door_1_1(p23);
-PwmOut Door_1_2(p24);
-PwmOut Door_2_1(p25);
-PwmOut Door_2_2(p26);
+MS5607I2C ms5607(p9, p10, false);
+MPU6050 mpu(p9,p10);
+BusOut myled(LED1,LED2,LED3,LED4);
+SDFileSystem sd(p11, p12, p13, p14, "sd");
+Serial device(USBTX, USBRX);
+PwmOut Door_1_1(p23);
+PwmOut Door_1_2(p24);
+PwmOut Door_2_1(p25);
+PwmOut Door_2_2(p26);
/*タイマー類*/
-Timer timer;
+Timer timer;
Ticker loop_log;
Ticker loop_open;
/*ログカウンタ*/
-bool row = 0;
+bool row = 0;
int8_t col = 0;
/*ログ格納用*/
float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG];
@@ -92,22 +89,23 @@
float t[2][RATE_LOG];
FILE *fp;
/*フェーズ変数*/
-int8_t Phase = STANDBY;
+int8_t Phase = SETUP;
+int8_t Mode = STANDBY;
/*判定用*/
-float alt4open[RATE_OPEN],acc4open;
-float alt_max,alt_launch;
-float t_drop,t_top,t_launch;
+float alt_buff[RATE_OPEN];
+float alt_max,alt_launch;
+float t_drop,t_top,t_launch;
int8_t cnt_judge = 0;
-int8_t col_open = 0;
+int8_t col_open = 0;
/*入力用*/
-int8_t Input_mode = NORMAL;
int8_t input_buff[3] = {};
+int8_t input_cnt = 0;
/*関数*/
-void _Open();
-void _Servo(int8_t door_num, float motion);
-void _Log();
-void _Input();
-float _GetAlt(float press, float temp);
+void _Open();
+void _Servo(int8_t door_num, float motion);
+void _Log();
+void _Input();
+float _Measure_Alt(float press, float temp);
float _Median(float data[], int num);
/*---------------------------------------------------------------------*/
@@ -129,16 +127,15 @@
void _Open(){
myled = 1 << (Phase-1);
switch (Phase) {
- case STANDBY://スタンバイモード（発射判定不可）
- // Phase = LAUNCH;
+ case SETUP://セットアップモード（発射判定不可）
break;
case LAUNCH://点火モード（発射判定可）
- acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
- alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature());
- if(acc4open>ACC_LAUNCH){
+ float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
+ alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
+ if(acc_buff>ACC_LAUNCH){
if(cnt_judge++==3){
- Phase = FLIGHT;
- alt_launch = _Median(alt4open, RATE_OPEN);
+ Phase = RISE;
+ alt_launch = _Median(alt_buff, RATE_OPEN);
cnt_judge = 0;
}
t_launch = timer.read();
@@ -147,10 +144,10 @@
if(timer.read()>t_launch+1.0) cnt_judge = 0;
}
break;
- case FLIGHT://飛翔中（パラシュート開放判定）
- alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature());
- if(alt4open[col_open]>alt_max) alt_max = alt4open[col_open];
- if(alt4open[col_open]<alt_max-DROP_TOP){
+ case RISE://上昇中（パラシュート開放判定）
+ alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
+ if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open];
+ if(alt_buff[col_open]<alt_max-TOP_DROP_AMOUNT){
if(cnt_judge++==3){
_Servo(1,UNLOCK);
Phase = DROP;
@@ -160,14 +157,14 @@
}else{
if(timer.read()>t_top+1.0) cnt_judge = 0;
}
- if(timer.read()-t_launch>TIME_TOP){
+ if(timer.read()-t_launch>TIME_REACH_TOP){
_Servo(1,UNLOCK);
Phase = DROP;
cnt_judge = 0;
}
break;
case DROP://降下中（缶サット開放判定)
- if(alt4open[col_open] < alt_launch+ALT_CAN){
+ if(alt_buff[col_open] < alt_launch+ALT_UAV_DROP){
if(cnt_judge++==3){
_Servo(2,UNLOCK);
cnt_judge = 0;
@@ -182,8 +179,8 @@
}
/*記録用関数 RATE_LOG[Hz]で記録を行う*/
void _Log(){
- t[row][col] = timer.read();
- pressure[row][col] = ms5607.getPressure()/100;
+ t[row][col] = timer.read();
+ pressure[row][col] = ms5607.getPressure()/100;
temperature[row][col] = ms5607.getTemperature();
mpu.getAccelero(&acc[row][col][0]);
mpu.getGyro(&gyro[row][col][0]);
@@ -193,9 +190,9 @@
);
if(col++==RATE_LOG){
fclose(fp);
- fp = fopen("/sd/log.txt", "a");
+ fp = fopen("/sd/log.txt", "a");
row =! row;
- col = 0;
+ col = 0;
}
}
/*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
@@ -217,72 +214,103 @@
}
/*入力用*/
+//TODO :テストモード実装
+//FIXME:android端末だと入力崩れが起こる
void _Input(){
- char c = device.getc();
- switch (Input_mode) {
- case NORMAL:
- if(c=='s'){
- Input_mode = SERVO_1;
- device.printf("\r\nInput_mode:SERVO\r\n");
- device.printf("\r\n0:All\r\n1:Parachute\r\n2:Cansat\r\n>>");
- }else if(c=='c'){
- Input_mode = MODE;
- device.printf("\r\nn:NORMAL\r\nf:FLIGHT\r\nt:TEST\r\n>>");
- }else{
- device.printf("\r\nthis command is not found:%c\r\n",c);
- device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
+ input_buff[input_cnt] = device.getc();
+ device.printf("\r\n");
+ device.printf("input_cnt:%d\r\n",input_cnt);
+ device.printf("input_buff:%s\r\n",input_buff);
+ switch (Mode) {
+ case STANDBY:
+ if(input_cnt==0){
+ if(input_buff[0]=='S'){
+ device.printf("P >> Parachute\r\n");
+ device.printf("C >> CanSat\r\n");
+ }else if(input_buff[0]=='M'){
+ device.printf("S >> STANDBY\r\n");
+ device.printf("T >> TEST\r\n");
+ device.printf("F >> FLIGHT\r\n");
+ }else{
+ device.printf("This command is not found >> %c\r\n",input_buff[0]);
+ device.printf(">>MAINMENU<<\r\n");
+ device.printf("S >> Servo Operation\r\n");
+ device.printf("M >> Mode Change\r\n");
+ device.printf("-->>");
+ return;
+ }
+ }else if(input_cnt==1){
+ if(input_buff[0]=='S'){
+ if(input_buff[1]=='P'||input_buff[1]=='C'){
+ device.printf("U >> UNLOCK\r\n");
+ device.printf("L >> LOCK\r\n");
+ }else{
+ device.printf("This command is not found >> %c\r\n",input_buff[1]);
+ device.printf("P >> Parachute\r\n");
+ device.printf("C >> CanSat\r\n");
+ device.printf("-->>");
+ return;
+ }
+ }else if(input_buff[0]=='M'){
+ if(input_buff[1]=='S'){
+ Mode = STANDBY;
+ device.printf(">>MAINMENU<<\r\n");
+ device.printf("S >> Servo Operation\r\n");
+ device.printf("M >> Mode Change\r\n");
+ device.printf("-->>");
+ }else if(input_buff[1]=='T'){
+ Mode = TEST;
+ }else if(input_buff[1]=='F'){
+ Mode = FLIGHT;
+ device.printf("FLIGHT-Mode ON!\r\n");
+ device.printf("***alert***\r\n");
+ device.printf("You will be able to reset only!\r\n");
+ }else{
+ device.printf("This command is not found >> %c\r\n",input_buff[1]);
+ device.printf("S >> STANDBY\r\n");
+ device.printf("T >> TEST\r\n");
+ device.printf("F >> FLIGHT\r\n");
+ device.printf("-->>");
+ return;
+ }
+ input_cnt = 0;
+ }
+ }else if(input_cnt==2){
+ if(input_buff[2]=='U'){
+ if(input_buff[1]=='P') _Servo(1,UNLOCK);
+ if(input_buff[1]=='C') _Servo(2,UNLOCK);
+ }else if(input_buff[2]=='L'){
+ if(input_buff[1]=='P') _Servo(1,LOCK);
+ if(input_buff[1]=='C') _Servo(2,LOCK);
+ }else{
+ device.printf("This command is not found >> %c\r\n",input_buff[2]);
+ device.printf("U >> UNLOCK\r\n");
+ device.printf("L >> LOCK\r\n");
+ device.printf("-->>");
+ return;
+ }
+ input_cnt = 0;
+ device.printf(">>MAINMENU<<\r\n");
+ device.printf("S >> Servo Operation\r\n");
+ device.printf("M >> Mode Change\r\n");
+ device.printf("-->>");
+ return;
}
- break;
- case SERVO_1:
- input_buff[0] = (int)(c-'0');
- if(c=='0'||c=='1'||c=='2'){
- Input_mode = SERVO_2;
- device.printf("\r\n0:OPEN\r\n1:CLOSE\r\n>>");
- }else{
- device.printf("\r\nthis servo_num is not found:%d\r\n",input_buff[0]);
- device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
- Input_mode = NORMAL;
- }
+ device.printf("-->>");
+ input_cnt++;
break;
- case SERVO_2:
- if(c=='0')_Servo(input_buff[0],UNLOCK);
- else if(c=='1')_Servo(input_buff[0],LOCK);
- else device.printf("\r\nthis motion_num is not found:%c\r\n",c);
- device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
- Input_mode = NORMAL;
+ case TEST:
break;
- case MODE:
- if(c=='n'){
- Input_mode = NORMAL;
- device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
- }else if(c=='f'){
- Input_mode = BAN;
- device.printf("\r\nyou will be able to reset only after this action!");
- fp = fopen("/sd/log.txt", "w");
- fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
- Phase = LAUNCH;
- loop_log.attach(&_Log,1.0/RATE_LOG);
- loop_open.attach(&_Open,1.0/RATE_OPEN);
- device.printf("\r\nFLIGHT-Mode ON!\r\n");
- }else if(c=='d'){
- Input_mode = DEBUG;
- }
- break;
- case DEBUG:
-
- break;
- case BAN://reset only
- break;
- default:
+ case FLIGHT://reset only
break;
}
}
/*その他雑関数*/
-float _GetAlt(float press, float temp){//高度計算
- return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){
+ return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}
-float _Median(float data[], int num){//中央値計算
+float _Median(float data[], int num){
float median;
float *sort = (float *)malloc(sizeof(float)*num);
for(int i=0; i<num; i++) sort[i] = data[i];