File content as of revision 4:345e55d8e8a3:

#include "mbed.h"
#include "IM920.h"
#include "GPS_interrupt.h"
#include "PQLPS22HB.h"
#include "mpu9250_i2c.h"
#include "INA226.h"
#include "Nichrome_lib.h"
#include "EEPROM_lib.h"

#define RESET_TIME 1800000 //1800000ms→30分
//#define RESET_TIME 30000000 // 30000000→30秒(実験用)
#define FIRE_TIME 2.0
#define TO_SEPARATE_TIME 8.0
//#define TO_SEPARATE_TIME 300.0
#define TO_COLLECTION_TIME 30.0
//#define TO_COLLECTION_TIME 10.0 // 実験用
#define ALTITUDE_LIMIT 50.0
//#define ALTITUDE_LIMIT 10.0
#define ACC_RANGE _16G
#define GYRO_RANGE _2000DPS
//#define LPF_COEFFICIENT_ALT 0.01
//#define LPF_COEFFICIENT_ALT 0.001
#define LPF_COEFFICIENT_ALT 0.05
#define LPF_COEFFICIENT_VEL 0.2

#define TEMP_MULTIPLIER 100
#define LPF_ALT_MULTIPLIER 100
#define LPF_VEL_MULTIPLIER 100

// ***************************************************
// コンストラクタ
// ***************************************************
Serial pc(USBTX, USBRX, 230400);
Serial gps_serial(p9, p10, 115200);
Serial im920_serial(p13, p14, 115200);

I2C i2c(p28, p27);

IM920 im920(im920_serial, pc, 115200);
GPS_interrupt gps(&gps_serial);
LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
mpu9250 mpu9250(i2c,AD0_HIGH);
myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND);
myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD);
Nichrome_lib nich(p20);
EEPROM_lib EEPROM(i2c, 4);

InterruptIn flightpin(p18);
DigitalOut nich_led(LED1);
DigitalOut im920_busy_led(LED2);
DigitalOut pinA(p21);
DigitalOut pinB(p22);
DigitalOut pinC(p23);

DigitalIn im920_busy(p15);

Ticker send_data;
Ticker save_data;
Ticker get_data;

Timer main_timer;

//Timeout to_separate_time;
//Timeout to_collection_time;
//Timeout fire_time;
Timeout task_timeout;
// ***************************************************
// 関数の宣言
// ***************************************************
void uplink();
void echoIM();
void PerformTask();
void FlightPinDetect();
void ChangeModeToSep();
void ChangeModeToCol();
void Separate();
void StopSeparate();
void SetSensor();
void GetData();
void SendLaunchTime();
void SendData();
void SaveData();
void setEEPROMGroup(int group_num);

// ***************************************************
// 無線のヘッダまとめ
// ***************************************************
const char HEADER_SETUP = 0x01;
// 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
//   1    1       1        1          1           1            1    

const char HEADER_DATA = 0x02;
// 0xA2, duration_main_time, duration_flight_time, mode, nich_status, flightpin_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep
//   1            4                      4           1         0.125         0.125       4    4      4      4      4       4            4             4            4             4
const char HEADER_LAUNCH_TIME = 0x03;

const char HEADER_ECHO = 0x05;
// 0xA5,コマンド
//   1     1

// ***************************************************
// 変数の宣言
// ***************************************************
float launch_time = 0;
float previous_main_time = 0;
float flight_time = 0;
float main_time = 0;
int time_reset_counter = 0;

bool send_launch_time_status = false;

bool top_detect = false;
char bitshift_top_detect;

bool nich_status = false;
char bitshift_nich_status;

bool flightpin_status = false;
char bitshift_flightpin_status;

bool save_data_status = false;
char bitshift_save_data_status;

char bitshift_sum;

bool header_set = false;
char im_buf[55];//16なのって意味ある？
int im_buf_n = 0;

float lat, lon, height;
int satellite_number;
float press, temp;
float altitude;
//float ground_press = 1013.0,ground_temp = 25.0;
float ground_press = -1.0,ground_temp = -1.0;

float velocity;
float previous_lpf_velocity;
float lpf_velocity;

float previous_lpf_altitude;
float lpf_altitude;

float imu[6], mag[3];
float mag_geo[3];

float voltage_main, voltage_sep;
float current_main, current_sep;

int ptr, n = 0;
int eeprom_ptr = 0;
int eeprom_group_counter = 0;
int eeprom_number = 0;

const char MODE_SETUP = 0x00;
const char MODE_SAFE = 0x01;
const char MODE_TO_ONCALL = 0x02;
const char MODE_ONCALL = 0x03;
const char MODE_TO_FLIGHT = 0x04; 
const char MODE_FLIGHT = 0x05;
const char MODE_TO_SEPARATE = 0x06; 
const char MODE_SEPARATE = 0x07;
const char MODE_TO_DESCEND = 0x08;
const char MODE_DESCEND = 0x09;
const char MODE_TO_COLLECTION = 0x0a;
const char MODE_COLLECTION = 0x0b;

char mode = MODE_SETUP;

// ***************************************************
// メイン関数
// ***************************************************
int main() {
    pc.printf("Hello Main!\r\n");
    im920.attach(&uplink, 0xF0);
    send_data.attach(&SendData, 1.0);
    get_data.attach(&GetData, 0.02);

    while(1){
        PerformTask();
        /*
        if(im920_busy){
            im920_busy_led = 1;
        }else{
            im920_busy_led = 0;
        }
        */
    }
}

// ***************************************************
// アップリンク（地上局から）
// ***************************************************
void uplink(){
    echoIM();
    switch(im920.data[1]){
        
        case 'U':
        if(mode == MODE_FLIGHT || mode == MODE_ONCALL){
            mode = MODE_SETUP;
            save_data.detach();
            save_data_status = false;
            pc.printf("********************\r\n");
            pc.printf("Set : MODE_SETUP\r\n");
            pc.printf("********************\r\n\r\n");
        }
        break;
        
        case 'O':
        if(mode == MODE_SAFE){
            mode = MODE_TO_ONCALL;
            pc.printf("********************\r\n");
            pc.printf("Set : MODE_TO_ONCALL\r\n");
            pc.printf("********************\r\n\r\n");
        }
        break;
        
        case 'F':
        if(mode == MODE_ONCALL){
            mode = MODE_TO_FLIGHT;
            pc.printf("********************\r\n");
            pc.printf("Set : MODE_TO_FLIGHT\r\n");
            pc.printf("********************\r\n\r\n");
        }
        break;
        
        case 'S':
        if(mode == MODE_FLIGHT){
            mode = MODE_TO_SEPARATE;
            pc.printf("********************\r\n");
            pc.printf("Set : MODE_TO_SEPARATE\r\n");
            pc.printf("********************\r\n\r\n");
        }
        break;
        
        case 0x01:
        pc.printf("********************\r\n");
        pc.printf("SEPARATE\r\n");
        pc.printf("********************\r\n\r\n");
        Separate();
        break;
        
        case 0x00:
        pc.printf("********************\r\n");
        pc.printf("STOP SEPARATE\r\n");
        pc.printf("********************\r\n\r\n");
        StopSeparate();
        break;
    }
}

// ***************************************************
// 無線信号の送り返し
// ***************************************************
void echoIM(){
    im920.header(HEADER_ECHO);
    im920.write(im920.data[1]);
    im920.send();
}

// ***************************************************
// タスクの実行
// ***************************************************
void PerformTask(){
    switch(mode){
        
        case(MODE_SETUP):
        SetSensor();
        main_timer.start();
        send_data.detach();
        send_data.attach(&SendData, 1.0);
        mode = MODE_SAFE;
        break;
        
        case(MODE_SAFE):
        break;
        
        case(MODE_TO_ONCALL):
        ground_press = press;
        save_data.detach();
        save_data.attach(&SaveData, 1.0);
        save_data_status = true;
        flightpin.rise(&FlightPinDetect);
        mode = MODE_ONCALL;
        break;
        
        case(MODE_ONCALL):
        break;
        
        case(MODE_TO_FLIGHT):
        save_data.detach();
        save_data.attach(&SaveData, 0.02);
        launch_time = main_time;
        //to_separate_time.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
        task_timeout.detach();
        task_timeout.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
        mode = MODE_FLIGHT;
        break;
        
        case(MODE_FLIGHT):
        //pc.printf("%f\t%f\t%f\t%f\t%f\r\n",press,altitude,lpf_altitude, ALTITUDE_LIMIT,velocity);
        if(!im920_busy && !send_launch_time_status){
            SendLaunchTime();
        }
        if((lpf_altitude >= ALTITUDE_LIMIT) && (lpf_velocity < 0.0)){//and 速度＜0を書き込む
            mode = MODE_TO_SEPARATE;
            top_detect = true;
        }
        break;
        
        case(MODE_TO_SEPARATE):
        Separate();
        mode = MODE_SEPARATE;
        break;
        
        case(MODE_SEPARATE):
        if(nich.status == false){
            mode = MODE_TO_DESCEND;
        }
        break;
        
        case(MODE_TO_DESCEND):
        //to_collection_time.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
        task_timeout.detach();
        task_timeout.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
        mode = MODE_DESCEND;
        break;
        
        case(MODE_DESCEND): 
        break;
        
        case(MODE_TO_COLLECTION):
        save_data.detach();
        save_data.attach(&SaveData, 1.0);
        mode = MODE_COLLECTION;
        break;
        
        case(MODE_COLLECTION):
        break;  
    }
}

// ***************************************************
// フライトピン検知
// ***************************************************
void FlightPinDetect(){
    if(mode == MODE_ONCALL){
        mode = MODE_TO_FLIGHT;
        flightpin_status = true;
        /*
        pc.printf("********************\r\n");
        pc.printf("Set : MODE_TO_FLIGHT\r\n");
        pc.printf("********************\r\n\r\n");
        */
    }
}

// ***************************************************
// 1フライトモード→セパレートモード
// ***************************************************
void ChangeModeToSep(){
    if(mode == MODE_FLIGHT){
        mode = MODE_TO_SEPARATE;
        /*
        pc.printf("********************\r\n");
        pc.printf("Set : MODE_TO_SEPARATE\r\n");
        pc.printf("********************\r\n\r\n");
        */
    }
}

// ***************************************************
// 下降モード→回収モード
// ***************************************************
void ChangeModeToCol(){
    if(mode == MODE_DESCEND){
        mode = MODE_TO_COLLECTION;
        /*
        pc.printf("********************\r\n");
        pc.printf("Set : MODE_TO_COLLECTION\r\n");
        pc.printf("********************\r\n\r\n");
        */
    }
}

// ***************************************************
// 分離実行
// ***************************************************
void Separate(){
    nich.fire_on();
    nich_led = 1;
    //fire_time.attach(&StopSeparate,FIRE_TIME);
    task_timeout.detach();
    task_timeout.attach(&StopSeparate,FIRE_TIME);
}

void StopSeparate(){
     nich.fire_off();
     nich_led = 0;
}

// ***************************************************
// センサーのセットアップ
// ***************************************************
void SetSensor(){
    pc.printf("\r\n");
    for(int i = 0; i < 20; i++){
    pc.printf("*");
    }
    pc.printf("\r\n");
    pc.printf("Start Setting\r\n");
    
    if(!header_set){
        im920.header((char)HEADER_SETUP);
        header_set = true;
    }
    
    //FlightPin
    flightpin.mode(PullUp);
    
    //GPS
    if(gps.gps_readable == true){
        pc.printf("GPS         : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("GPS         : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
    }
    
    //LPS22HB
    lps22hb.begin();
    if(lps22hb.test() == true){
        pc.printf("LPS22HB     : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("LPS22HB     : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
    }
    
    //MPU9250
    if(mpu9250.sensorTest() == true){
        pc.printf("MPU9250     : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("MPU9250     : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
    }
    if(mpu9250.mag_sensorTest() == true){
        pc.printf("MPU9250 MAG : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("MPU9250 MAG : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
    }
    mpu9250.setAcc(ACC_RANGE);
    mpu9250.setGyro(GYRO_RANGE);
    mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012);

    //INA226
    ina226_main.set_callibretion();
    ina226_sep.set_callibretion();
    //ina226_main.setup(1);
    //ina226_sep.setup(1);
    
    if(ina226_main.Connection_check()==0){
        pc.printf("INA226 Main : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("INA226 Main : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
        }
    if(ina226_sep.Connection_check()==0){
        pc.printf("INA226 Sep  : OK!\r\n");
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
    }else{
        pc.printf("INA226 Sep  : NG...\r\n");
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
    }
    
    if(header_set){
        im920.send();
        pc.printf("Send : %s\r\n", im_buf);
        header_set = false;
        for(int i = 0; i < im_buf_n; i ++){
            im_buf[i] = '\0';
        }
        im_buf_n = 0;
    }
    
    //EEPROM
    eeprom_group_counter = 0;
    setEEPROMGroup(eeprom_group_counter);
    EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
    
    pc.printf("\r\n");
    for(int i = 0; i < 20; i++){
    pc.printf("*");
    }
    pc.printf("\r\n");
}

// ***************************************************
// センサーのデータ取得
// ***************************************************
void GetData(){
        
    //Main_Time
    previous_main_time = main_time;
    main_time = main_timer.read_ms();
    if(main_time >= RESET_TIME){
        main_timer.reset();
        time_reset_counter++;
    }
    
    //Nichrome
    nich_status = nich.status;
    
    //GPS
    lat = gps.Latitude();
    lon = gps.Longitude();
    height = gps.Height();
    satellite_number = gps.Number();
    //pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height);
    
    //LPS22HB
    float press_tmp;
    //lps22hb.read_press(&press);
    lps22hb.read_press(&press_tmp);
    
    if(press_tmp > 500.0){
        press = press_tmp;
    }
    
    if(ground_press == -1.0){
        ground_press = press;   
    }
    
    lps22hb.read_temp(&temp);
    
    if(ground_temp == -1.0){
        ground_temp = temp;   
    }
    
    altitude =  (ground_temp + 273.15)/0.0065*(1 - powf(press/ground_press, 287 * 0.0065/9.80665));
    //pc.printf("%f\t%f\t%f\r\n",press, temp, altitude);
    
    //lpf_altitude
    previous_lpf_altitude = lpf_altitude;
    lpf_altitude += LPF_COEFFICIENT_ALT*(altitude - previous_lpf_altitude);
    
    //pc.printf("%f\t%f\t\r\n",altitude,lpf_altitude);
    
    //velocity計算
    velocity = (lpf_altitude - previous_lpf_altitude)*1000/(main_time - previous_main_time);
    //pc.printf("********************\r\n");
    //pc.printf("lpf altitude    : %f\r\n",lpf_altitude);
    //pc.printf("main time    : %f\r\n",main_time/1000);
    //pc.printf("altitude def: %f\r\n",lpf_altitude - previous_lpf_altitude);
    //pc.printf("velocity    : %f\r\n",velocity);
    //pc.printf("time def: %f\r\n",main_time - previous_main_time);
    //pc.printf("********************\r\n");
    
    //lpf_velocity
    previous_lpf_velocity = lpf_velocity;
    lpf_velocity += LPF_COEFFICIENT_VEL*(velocity - previous_lpf_velocity);
    //pc.printf("%f\t%f\t%f\t%f\r\n",altitude,lpf_altitude,velocity,lpf_velocity);
    
    //MPU9250
    mpu9250.getAll(imu, mag);
    //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);
    
    //INA226
    ina226_main.get_Voltage_current(&voltage_main, &current_main);
    ina226_sep.get_Voltage_current(&voltage_sep, &current_sep);
    //pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main, voltage_sep, current_main, current_sep);
    //pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep);
}

// ***************************************************
// 取得データを地上に送信
// ***************************************************
void SendData(){
    if(!header_set){
        im920.header((char)HEADER_DATA);
        header_set = true;
    }
    
    ///pc.printf("********************\r\n");
    
    //time_reset_counter
    im920.write((short)time_reset_counter);
    im_buf_n += 2;
    //MainTime
    im920.write((float)main_time);
    im_buf_n += 4;
      
    //mode
    ///pc.printf("mode            : %c\r\n",mode);
    im920.write((char)mode);
    im_buf_n ++;    
    
    //flightpin
    bitshift_flightpin_status = flightpin_status << 0;
    //pc.printf("flightpin bool  : %d\r\n",flightpin_status);
    //pc.printf("flightpin bool  : %d\r\n",bitshift_flightpin_status);
    
    //Nichrome
    bitshift_nich_status = nich_status << 1;
    //pc.printf("nich bool      : %d\r\n",nich_status);
    //pc.printf("nich bitshift  : %d\r\n",bitshift_nich_status);
    
    //頂点検知
    bitshift_top_detect = top_detect << 2;
    
    //保存データのステータス
    bitshift_save_data_status = save_data_status << 3;
    
    //bool8個を1つのcharで送るとき
    bitshift_sum = bitshift_flightpin_status | bitshift_nich_status | bitshift_top_detect | bitshift_save_data_status;
    im920.write((char)bitshift_sum);
    im_buf_n ++;
    
    //EEPROM Number
    im920.write((char)eeprom_number);
    im_buf_n ++;
    
    //GPS
    ///pc.printf("Latitude        : %f\r\n",lat);
    ///pc.printf("Longitude       : %f\r\n",lon);
    ///pc.printf("Height          : %f\r\n",height);
    im920.write((float)lat);
    im_buf_n += 4;
    im920.write((float)lon);
    im_buf_n += 4;
    /*
    im920.write((float)height);
    im_buf_n += 4;
    */
    
    //LPS22HB
    ///pc.printf("Pressure        : %f\r\n",press);
    ///pc.printf("Temperarure     : %f\r\n",temp);
    ///pc.printf("Altitude        : %f\r\n",altitude);
    im920.write((float)press);
    im_buf_n += 4;
    im920.write((short)(temp*TEMP_MULTIPLIER));
    im_buf_n += 2;
    im920.write((short)(lpf_altitude*LPF_ALT_MULTIPLIER));
    im_buf_n += 2;
    
    im920.write((short)(lpf_velocity*LPF_VEL_MULTIPLIER));
    im_buf_n += 2;
    
    //INA226
    ///pc.printf("Vlotage Mian    : %.2f\r\n",voltage_main);
    ///pc.printf("Current Main    : %.2f\r\n",current_main);
    ///pc.printf("Voltage Sep     : %.2f\r\n",voltage_sep);
    ///pc.printf("Current Sep     : %.2f\r\n",current_sep);
    im920.write((short)voltage_main);
    im_buf_n += 2;
    im920.write((short)current_main);
    im_buf_n += 2;
    im920.write((short)voltage_sep);
    im_buf_n += 2;
    im920.write((short)current_sep);
    im_buf_n += 2;
    
     ///pc.printf("********************\r\n\r\n");
    
    if(header_set){
        im920.send();
        //pc.printf("Send : %s\r\n", im_buf);
        header_set = false;
        for(int i = 0; i < im_buf_n; i ++){
            im_buf[i] = '\0';
        }
        im_buf_n = 0;
    }
}

// ***************************************************
// 打上げ時刻を送信
// ***************************************************
void SendLaunchTime(){
    if(!header_set){
        im920.header((char)HEADER_LAUNCH_TIME); 
        header_set = true;
    }
    pc.printf("launch time : %f\r\n",launch_time); 
    im920.write((float)launch_time);
    im_buf_n += 4;
    im920.write((short)time_reset_counter);
    im_buf_n += 2;
    send_launch_time_status = true;
    if(header_set){
        im920.send();
        pc.printf("Send launch time\r\n");
        header_set = false;
        for(int i = 0; i < im_buf_n; i ++){
            im_buf[i] = '\0';
        }
        im_buf_n = 0;
    }
}

// ***************************************************
// EEPROMにデータを書き込む
// ***************************************************
void SaveData(){
    if(eeprom_group_counter < 4){
        //pc.printf("Save to EEPROM\r\n");
        int eep_buf = 0;
        
        /*
        for(int i = eep_buf; i < 128; i++){
            //ptr = EEPROM.chargeBuff((char)0x02);
            ptr = EEPROM.chargeBuff((int)n++);
        }
        */
        ptr = EEPROM.chargeBuff((char)time_reset_counter);
        eep_buf += 1;
        ptr = EEPROM.chargeBuff((int)main_time);
        eep_buf += 4;
        //ptr = EEPROM.chargeBuff((float)flight_time);
        //eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((float)lat);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)lon);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)height);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((char)satellite_number);
        eep_buf += 1;
        
        ptr = EEPROM.chargeBuff((float)press);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)temp);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)altitude);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)lpf_altitude);
        eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((float)velocity);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)lpf_velocity);
        eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((float)imu[0]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)imu[1]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)imu[2]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)imu[3]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)imu[4]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)imu[5]);
        eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((float)mag[0]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)mag[1]);
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)mag[2]);
        eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((float)(voltage_main/1000));
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)(current_main/1000));
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)(voltage_sep/1000));
        eep_buf += 4;
        ptr = EEPROM.chargeBuff((float)(current_sep/1000));
        eep_buf += 4;
        
        ptr = EEPROM.chargeBuff((char)mode);
        eep_buf ++;
        ptr = EEPROM.chargeBuff((char)flightpin_status);
        eep_buf ++;
        ptr = EEPROM.chargeBuff((char)nich_status);
        eep_buf ++;
        
        //ptr = EEPROM.chargeBuff((int)n++);
        //ptr = EEPROM.chargeBuff((char)0xff);
        //ptr = EEPROM.chargeBuff((int)0);
        
        //pc.printf("%d\r\n",eep_buf);
        
        for(int i = eep_buf; i < 128; i++){
            ptr = EEPROM.chargeBuff((char)0x00);
        }
        
        //pc.printf("ptr : %d\r\n",ptr);
        EEPROM.writeBuff();
        eeprom_ptr = EEPROM.setNextPage();
        if(eeprom_ptr == 0x01000000 || eeprom_ptr == 0x02000000 || eeprom_ptr == 0x03000000 || eeprom_ptr == 0x04000000){
            eeprom_number++;
            pc.printf("eeprom_number : %d\r\n",eeprom_number);
        }
        //pc.printf("eeprom_ptr: %x\r\n", eeprom_ptr);
        
        /*
        if(eeprom_ptr == 0x01000000){
            ptr = 0;
            eeprom_ptr = 0;
            plexer_num++;
            setEEPROMGroup(plexer_num);
            EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
        }
        */
        
        if(eeprom_ptr == 0x04000000){
            eeprom_ptr = 0;
            eeprom_group_counter++;
            setEEPROMGroup(eeprom_group_counter);
            EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
            pc.printf("EEPROM_Group : %d\r\n",eeprom_group_counter);
            //pc.printf("SetWriteAddr\r\n");
        }
    }
}

// ***************************************************
// マルチプレクサで使うEEPROMを変更する
// ***************************************************
void setEEPROMGroup(int group_num){
    switch(group_num){
        case 0:
        pinA = 0;
        pinB = 0;
        pinC = 0;
        break;
        
        case 1:
        pinA = 1;
        pinB = 0;
        pinC = 0;
        break;
        
        case 2:
        pinA = 0;
        pinB = 1;
        pinC = 0;
        break;
        
        case 3:
        pinA = 1;
        pinB = 1;
        pinC = 0;
        break;
    }
}