File content as of revision 17:5f7808444d96:

/*
2017年3月　伊豆大島共同打上実験
団体名：CORE
チーム名：ヌペリオル
該当電装：ロケット搭載用

使用部品
・LPC1768(マイコン)
・MPU6050(加速度・ジャイロセンサ)
・MS5607(気圧・気温センサ)
・MicroSDスロット
・MG995(サーボモータ)×4

使用ライブラリ
・MPU6050.h
   https://developer.mbed.org/teams/mbed/code/SDFileSystem/
・MS5607.h
   https://developer.mbed.org/users/yamaguch/code/MS5607/
・SDFileSystem.h
   https://developer.mbed.org/teams/mbed/code/SDFileSystem/

ピン配置
LPC1768  MPU6050  MS5607  MicroSD　MG995_1  MG995_2  MG995_3  MG995_4
VIN--------------------------------------------------------------------4.5~9.0V
-----------------------------------VIN------VIN------VIN------VIN------5V
Vout-----VCC------VIN-----VDD------------------------------------------3.3V
       \-ADO    \-PS
GND------GND------GND-----VSS------GND------GND------GND------GND------0V
                \-CS
p9-------SDA------SDA----------------------------------------------------
p10------SCL------SCL----------------------------------------------------
p11-----------------------CLK--------------------------------------------
p12-----------------------DAT0-------------------------------------------
p13-----------------------CMD--------------------------------------------
p14-----------------------CD/DAT0----------------------------------------
p23--------------------------------CTRL----------------------------------
p24-----------------------------------------CTRL-------------------------
p25--------------------------------------------------CTRL----------------
p26-----------------------------------------------------------CTRL-------
*/
#include "mbed.h"
#include "math.h"
#include "MS5607I2C.h"
#include "MPU6050.h"
#include "SDFileSystem.h"

#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
/*サーボ動作*/
#define LOCK      0
#define UNLOCK    1
/*モード定義*/
#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//加速度オフセット値
/*ピン指定*/
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;
Ticker loop_log;
Ticker loop_open;
/*ログカウンタ*/
FILE *fp;
bool   row = 0;
int8_t col = 0;
/*ログ格納用*/
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];
/*フェーズ変数*/
int8_t Phase = SETUP;
int8_t Mode  = STANDBY;
/*判定用*/
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 input_buff[3] = {};
int8_t input_cnt = 0;
/*関数*/
void  _Open();
void  _Log();
void  _Servo(int8_t door_num, int8_t motion);
void  _Input();
float _Measure_Alt(float press, float temp);
float _Median(float data[], int num);

/*---------------------------------------------------------------------*/
int main() {
   mpu.setAcceleroRange(2);
   mpu.setGyroRange(2);
   timer.start();
   Door_1_1.period_ms(20);
   Door_1_2.period_ms(20);
   Door_2_1.period_ms(20);
   Door_2_2.period_ms(20);
   // _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定？
   // _Servo(2,UNLOCK);//
   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);
}
/*開放用関数　RATE_OPEN[Hz]で判定を行う*/
void _Open(){
   myled = 1 << (Phase-1);
   switch (Phase) {
      case SETUP://セットアップモード（発射判定不可）
                  break;
      case LAUNCH://点火モード（発射判定可）
                  float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
                  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;
                     }
                     t_launch = timer.read();
                     alt_max = alt_launch;
                  }else{
                     if(timer.read()>t_launch+1.0) cnt_judge = 0;
                  }
                  break;
      case RISE://上昇中（パラシュート開放判定）
                  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;
                        cnt_judge = 0;
                     }
                     t_top = timer.read();
                  }else{
                     if(timer.read()>t_top+1.0) cnt_judge = 0;
                  }
                  if(timer.read()-t_launch>TIME_REACH_TOP){
                     _Servo(2,UNLOCK);
                     Phase = DROP;
                     cnt_judge = 0;
                  }
                  break;
      case DROP://降下中（缶サット開放判定)
                  float alt_droping = ms5607.getAltitude();
                  if(alt_droping < alt_launch+ALT_UAV_DROP){
                     if(cnt_judge++==3){
                        Phase++;
                        _Servo(1,UNLOCK);
                        cnt_judge = 0;
                     }
                     t_drop = timer.read();
                  }else{
                     if(timer.read()>t_drop+1.0)cnt_judge = 0;
                  }
                  break;
   }
   if(col_open++==RATE_OPEN) col_open = 0;
}
/*記録用関数　RATE_LOG[Hz]で記録を行う*/
void _Log(){
   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,"%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");
   }
}
/*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/
void _Servo(int8_t door_num, int8_t motion){
   if(door_num==1){//扉１
      if(motion==UNLOCK){
          Door_1_1.pulsewidth(0.0015);
          wait(0.2);
          Door_1_2.pulsewidth(0.0015);
      }else if(motion==LOCK){
          Door_1_1.pulsewidth(0.0024);
          wait(0.2);
          Door_1_2.pulsewidth(0.0005);
      }else{
          device.printf("error%f\r\n",motion);
      }
   }else if(door_num==2){//扉２
      if(motion==UNLOCK){
         Door_2_1.pulsewidth(0.0015);
         wait(0.2);
         Door_2_2.pulsewidth(0.0015);
      }else if(motion==LOCK){
         Door_2_1.pulsewidth(0.0024);
         wait(0.2);
         Door_2_2.pulsewidth(0.0005);
      }else{
         device.printf("error%f\r\n",motion);
      }
   }else{
      device.printf("servo error:%d\r\n",door_num);
   }
}

/*入力用*/
// TODO:テストモード実装
// FIXME:android端末だと入力崩れが起こる
void _Input(){
   input_buff[input_cnt] = device.getc();
   device.printf("\r\n");
   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("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");
                        }else if(input_buff[1]=='F'){
                           Mode = FLIGHT;
                           Phase = LAUNCH;
                           loop_log.attach(&_Log,1.0/RATE_LOG);
                           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("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(2,UNLOCK);
                        if(input_buff[1]=='C') _Servo(1,UNLOCK);
                     }else if(input_buff[2]=='L'){
                        if(input_buff[1]=='P') _Servo(2,LOCK);
                        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("-->>");
                        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;
                  }
                  device.printf("-->>");
                  input_cnt++;
                  break;
      case FLIGHT://reset only
                  break;
   }
}

/*その他雑関数*/
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 *sort = (float *)malloc(sizeof(float)*num);
   for(int i=0; i<num; i++) sort[i] = data[i];
   for(int i=0; i<num; i++){
      for(int j=0; j<num-i-1; j++){
         if(sort[j]>sort[j+1]){
            float buff = sort[j+1];
            sort[j+1] = sort[j];
            sort[j] = buff;
         }
      }
   }
   if(num%2!=0)median = sort[num/2];
   else median = (sort[num/2-1]+sort[num/2])/2.0;
   free(sort);
   return median;
}