File content as of revision 5:bcf55d8fe7a7:

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

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

ピン配置
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 "MS5607I2C.h"
#include "MPU6050.h"
#include "SDFileSystem.h"

/*動作レート*/
#define RATE_LOG 10//ログ用
#define RATE_OPEN 10//開放用
/*サーボ動作*/
#define LOCK true
#define UNLOCK false
/*フェイズ定義*/
#define STANDBY 0
#define LAUNCH 1
#define FLIGHT 2
#define OPEN_1 3
#define OPEN_2 4

MS5607I2C ms5607(p9, p10, false);
MPU6050 mpu(p9,p10);
DigitalIn sw(p21);
BusOut myled(LED1,LED2,LED3,LED4);
Serial pc(USBTX, USBRX);
SDFileSystem sd(p11, p12, p13, p14, "sd");

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;

/*ログカウンタ*/
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];
float t_top,alt,alt_max,alt_launch;
/*フェイズ変数*/
int8_t Phase = STANDBY;
/*降下判定用*/
int8_t cnt_drop = 0;
FILE *fp;

void _Open();
void _Servo(int8_t door_num, bool door_motion);
void _Log();

int main() {
   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);
   fp = fopen("/sd/log.txt", "w");
   fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
   loop_log.attach(&_Log,1.0/RATE_LOG);
   loop_open.attach(&_Open,1.0/RATE_OPEN);
   while(1);
}

void _Open(){
   myled = 1 << (Phase-1);
   switch (Phase) {
      case STANDBY://スタンバイモード（発射判定不可）
                  Phase = LAUNCH;
                  break;
      case LAUNCH://フライトモード（発射判定可）
                  alt_launch = ms5607.getAltitude();
                  if(sw==0)Phase = FLIGHT;
                  break;
      case FLIGHT://飛翔中（頂点判定可）
                  alt=ms5607.getAltitude();
                  if(alt > alt_max) alt_max = alt;
                  else if(alt_max - alt > 1.5) Phase = OPEN_1;
                  t_top = timer.read();
                  break;
      case OPEN_1://パラシュート開放モード
                  if(alt_max>1.5) cnt_drop++;
                  if(cnt_drop==5){
                     _Servo(1,UNLOCK);
                     Phase = OPEN_2;
                     cnt_drop = 0;
                  }
                  break;
      case OPEN_2://缶サット開放モード
                  if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
                  if (cnt_drop==5) _Servo(2,UNLOCK);
                  break;
   }
}

void _Log(){
   t[row][col] = timer.read();
   pressure[row][col] = ms5607.getPressure();
   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],
               acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
            );
   if(col++==RATE_LOG){
      fclose(fp);
      fp = fopen("/sd/log.txt", "a");
      row =! row;
      col = 0;
    }
}

void _Servo(int8_t door_num, bool door_motion){
   
}