CORE
2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 17:5f7808444d96
- Parent:
- 16:0a239047eb29
- Child:
- 18:5fb8ce45ce51
--- a/main.cpp Mon Mar 13 12:41:09 2017 +0000
+++ b/main.cpp Tue Mar 14 12:13:28 2017 +0000
@@ -44,10 +44,10 @@
#include "MPU6050.h"
#include "SDFileSystem.h"
-#define ACC_LAUNCH 0//FIXME:本番は4g
-#define TOP_DROP_AMOUNT 1.5
-#define ALT_UAV_DROP 1.0//FIXME:本番は150m
-#define TIME_REACH_TOP 15
+#define ACC_LAUNCH 4.0f//FIXME:本番は4g
+#define TOP_DROP_AMOUNT 1.5f
+#define ALT_UAV_DROP 150.0f//FIXME:本番は150m
+#define TIME_REACH_TOP 500.0f//FIXME:本番は15sec
#define RATE_LOG 10
#define RATE_OPEN 10
@@ -81,13 +81,13 @@
Ticker loop_log;
Ticker loop_open;
/*ログカウンタ*/
+FILE *fp;
bool row = 0;
int8_t col = 0;
/*ログ格納用*/
-float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG];
+float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
float t[2][RATE_LOG];
-FILE *fp;
/*フェーズ変数*/
int8_t Phase = SETUP;
int8_t Mode = STANDBY;
@@ -102,8 +102,8 @@
int8_t input_cnt = 0;
/*関数*/
void _Open();
+void _Log();
void _Servo(int8_t door_num, int8_t motion);
-void _Log();
void _Input();
float _Measure_Alt(float press, float temp);
float _Median(float data[], int num);
@@ -119,8 +119,8 @@
Door_2_2.period_ms(20);
// _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定？
// _Servo(2,UNLOCK);//
- fp = fopen("/sd/log.txt", "w");
- fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
+ fp = fopen("/sd/log.txt", "a");
+ fprintf(fp, "Phase,time,pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
device.attach(&_Input,Serial::RxIrq);
loop_open.attach(&_Open,1.0/RATE_OPEN);
while(1);
@@ -128,18 +128,17 @@
/*開放用関数 RATE_OPEN[Hz]で判定を行う*/
void _Open(){
myled = 1 << (Phase-1);
- device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max);
switch (Phase) {
case SETUP://セットアップモード（発射判定不可）
break;
case LAUNCH://点火モード（発射判定可）
float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
- alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
- if(acc_buff>(float)ACC_LAUNCH){
+ alt_buff[col_open] = ms5607.getAltitude();
+ if(acc_buff>ACC_LAUNCH){
if(cnt_judge++==3){
- Phase = RISE;
- alt_launch = _Median(alt_buff, RATE_OPEN);
- cnt_judge = 0;
+ Phase = RISE;
+ alt_launch = _Median(alt_buff, RATE_OPEN);
+ cnt_judge = 0;
}
t_launch = timer.read();
alt_max = alt_launch;
@@ -148,9 +147,9 @@
}
break;
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-(float)TOP_DROP_AMOUNT){
+ float alt_rising = ms5607.getAltitude();
+ if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising;
+ if(alt_rising<alt_max-TOP_DROP_AMOUNT){
if(cnt_judge++==3){
_Servo(2,UNLOCK);
Phase = DROP;
@@ -160,16 +159,15 @@
}else{
if(timer.read()>t_top+1.0) cnt_judge = 0;
}
- /*FIXME:当日は有効化
if(timer.read()-t_launch>TIME_REACH_TOP){
_Servo(2,UNLOCK);
Phase = DROP;
cnt_judge = 0;
}
- */
break;
case DROP://降下中（缶サット開放判定)
- if(alt_buff[col_open] < alt_launch+(float)ALT_UAV_DROP){
+ float alt_droping = ms5607.getAltitude();
+ if(alt_droping < alt_launch+ALT_UAV_DROP){
if(cnt_judge++==3){
Phase++;
_Servo(1,UNLOCK);
@@ -185,21 +183,24 @@
}
/*記録用関数 RATE_LOG[Hz]で記録を行う*/
void _Log(){
- t[row][col] = timer.read();
+ if(t[row][col] = timer.read()>=30.0*60.0){
+ timer.reset();
+ 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]);
- fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
- t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0,
+ fprintf(fp,"%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
+ Phase,t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0,
acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
);
if(col++==RATE_LOG){
+ col = 0;
+ row =! row;
fclose(fp);
fp = fopen("/sd/log.txt", "a");
- row =! row;
- col = 0;
- }
+ }
}
/*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/
void _Servo(int8_t door_num, int8_t motion){
@@ -238,35 +239,32 @@
void _Input(){
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");
+ 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");
+ device.printf("S : STANDBY\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("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");
+ 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("P : Parachute\r\n");
+ device.printf("C : CanSat\r\n");
device.printf("-->>");
return;
}
@@ -274,13 +272,8 @@
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;
- Phase = RISE;
- alt_launch = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
+ device.printf("S : Servo Operation\r\n");
+ device.printf("M : Mode Change\r\n");
}else if(input_buff[1]=='F'){
Mode = FLIGHT;
Phase = LAUNCH;
@@ -290,9 +283,8 @@
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("S : STANDBY\r\n");
+ device.printf("F : FLIGHT\r\n");
device.printf("-->>");
return;
}
@@ -307,23 +299,21 @@
if(input_buff[1]=='C') _Servo(1,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("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("S : Servo Operation\r\n");
+ device.printf("M : Mode Change\r\n");
device.printf("-->>");
return;
}
device.printf("-->>");
input_cnt++;
break;
- case TEST:
- break;
case FLIGHT://reset only
break;
}