File content as of revision 2:41238982b498:

#include "mbed.h"
#include "IM920.h"

#define RESET_TIME 1800

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

// ***************************************************
// コンストラクタ
// ***************************************************
Serial pc(USBTX, USBRX, 115200);
Serial im920_serial(D1, D0, 115200);

IM920 im920(im920_serial, pc, 115200);

// ***************************************************
// 関数の宣言
// ***************************************************
void ReadPC();
void ReadSensorSetup();
void ReadData();
void ReadLaunchTime();
void Read0xA5();
void Help();

// ***************************************************
// 変数の宣言
// ***************************************************
int minute;
int second;
int launch_minute;
int launch_second;
int flight_minute = 0;
int flight_second = 0;
float flight_time = 0;
float duration_flight_time = 0;
float main_time;
float duration_main_time = 0;
float launch_time = 0;
int time_reset_counter = 0;
int time_reset_counter_launch = 0;

char mode;

bool top_detect;
bool flightpin_status;
bool nich_status;
bool save_data_status;
char bitshift_sum;

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

char eeprom_number;

float lat, lon, height;
float press, temp, lpf_altitude;
float lpf_velocity;

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

float voltage_main, voltage_sep;
float current_main, current_sep;

// ***************************************************
// 無線のヘッダまとめ
// ***************************************************
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

// ***************************************************
// メイン関数
// ***************************************************
int main() {
    //wait(0.5f);
    pc.printf("Hello Ground!\r\n");
    pc.attach(&ReadPC, Serial::RxIrq);
    im920.attach(&ReadLaunchTime, HEADER_LAUNCH_TIME);
    im920.attach(&Read0xA5, HEADER_ECHO);
    im920.attach(&ReadSensorSetup, HEADER_SETUP);
    im920.attach(&ReadData, HEADER_DATA);
}

void ReadPC(){
    char c = pc.getc();
    if(!header_set){
        im920.header((char)0xF0);
        header_set = true;
    }
    
    switch(c){
        case 'U':
        im920.write((char)c);
        im_buf[im_buf_n] = 'U';
        im_buf_n ++;
        pc.printf("INPUT : U\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;
        }
        break;
        
        case 'O':
        im920.write((char)c);
        im_buf[im_buf_n] = 'O';
        im_buf_n ++;
        pc.printf("INPUT : O\r\n");
        break;
        
        case 'F':
        im920.write((char)c);
        im_buf[im_buf_n] = 'F';
        im_buf_n ++;
        pc.printf("INPUT : F\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;
        }
        break;
        
        case 'S':
        im920.write((char)c);
        im_buf[im_buf_n] = 'S';
        im_buf_n ++;
        pc.printf("INPUT : S\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;
        }
        break;
        
        case '0':
        im920.write((char)0x00);
        im_buf[im_buf_n] = '0';
        im_buf_n ++;
        pc.printf("INPUT : 0\r\n");
        break;
        
        case '1':
        im920.write((char)0x01);
        im_buf[im_buf_n] = '1';
        im_buf_n ++;
        pc.printf("INPUT : 1\r\n");
        break;
        
        case '?':
        Help();
        break;
        
        case '\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;
        }
        break;
    }
}

void ReadSensorSetup(){
    pc.printf("\r\n");
    pc.printf("****Sensor Setup****\r\n");
    
    if(im920.data[1] == 1){
        pc.printf("GPS         : OK\r\n");   
    }else{
        pc.printf("GPS         : NG\r\n");
    }
    if(im920.data[2] == 1){
        pc.printf("LPS22HB     : OK\r\n");
    }else{
        pc.printf("LPS22HB     : NG\r\n");  
    }
    if(im920.data[3] == 1){
        pc.printf("MPU9250     : OK\r\n");
    }else{
        pc.printf("MPU9250     : NG\r\n");
    }
    if(im920.data[4] == 1){
        pc.printf("MPU9250_MAG : OK\r\n");
    }else{
        pc.printf("MPU9250_MAG : NG\r\n");
    }
    if(im920.data[5] == 1){
        pc.printf("INA226_MAIN : OK\r\n");
    }else{
        pc.printf("INA226_MAIN : NG\r\n");
    }
    if(im920.data[6] == 1){
        pc.printf("INA226_SEP  : OK\r\n");
    }else{
        pc.printf("INA226_SEP  : NG\r\n");
    }
    
    for(int i = 0; i < 20; i++){
     pc.printf("*");   
    }
    pc.printf("\r\n");
}

void ReadData(){
    //pc.printf("***Acquired Data****\r\n");
    pc.printf("***Recieved Data****\r\n");
    
    int bit = 1;
    
    time_reset_counter = (short)im920.toShort(bit);
    bit += 2;
    
    main_time = (float)im920.toFloat(bit);
    main_time /= 1000;
    bit += 4;
    
    minute = main_time / 60 + time_reset_counter*30;
    //pc.printf("%d\t%f\r\n",minute,minute);
    second = (int)main_time % 60;
    
    //duration_main_time = main_time + RESET_TIME*time_reset_counter;
    pc.printf("Main Time       : %d : %02d\r\n",minute,second);
    //pc.printf("Main Time      : %f\r\n",duration_main_time);
    
    pc.printf("Launch Time     : %d : %02d\r\n",launch_minute,launch_second);
      
    if(mode >= 0x04){
            if(second >= launch_second){
                flight_minute = minute - launch_minute;
                flight_second = second - launch_second;
            }else{
                flight_minute = minute - launch_minute - 1;
                flight_second = 60 + second - launch_second;
            }
            pc.printf("Flight Time     : %d : %02d\r\n",flight_minute,flight_second);
    }else{
        pc.printf("Flight Time     : %d : %02d\r\n",flight_minute,flight_second);   
    }
    
    mode = im920.data[bit];
    bit += 1;
    switch(mode){
        case 0x00:
        pc.printf("Mode            : SETUP\r\n");
        break;
        
        case 0x01:
        pc.printf("Mode            : SAFETY\r\n");
        break;
        
        case 0x02:
        pc.printf("Mode            : TO_ONCALL\r\n");
        break;
        
        case 0x03:
        pc.printf("Mode            : ONCALL\r\n");
        break;
        
        case 0x04:
        pc.printf("Mode            : TO_FLIGHT\r\n");
        break;
        
        case 0x05:
        pc.printf("Mode            : FLIGHT\r\n");
        break;
        
        case 0x06:
        pc.printf("Mode            : TO_SEPARATE\r\n");
        break;
        
        case 0x07:
        pc.printf("Mode            : SEPARATE\r\n");
        break;
        
        case 0x08:
        pc.printf("Mode            : TO_DESCEND\r\n");
        break;
        
        case 0x09:
        pc.printf("Mode            : DESCEND\r\n");
        break;
        
        case 0x0a:
        pc.printf("Mode            : TO_COLLECTION\r\n");
        break;
        
        case 0x0b:
        pc.printf("Mode            : COLLECTION\r\n");
        break;
    }
    
    /*bool1個を1バイトで送る
    flightpin_status = im920.data[bit];
    bit += 1;        
    pc.printf("FlightPin Status: %d\r\n",flightpin_status);
    
    nich_status = im920.data[bit];
    bit += 1;
    pc.printf("Nichrome Status : %d\r\n",nich_status);
    */
    
    //bool8個を1バイトで送る
    bitshift_sum = im920.data[bit];
    bit += 1;
    //pc.printf("Bitshift Sum    : %d\r\n",bitshift_sum);
    
    flightpin_status = 0x01 & (bitshift_sum); 
    pc.printf("FlightPin Status: %d\r\n",flightpin_status);
    
    //nich_status = bitshift_sum >> 1;
    nich_status = 0x02 & (bitshift_sum);
    pc.printf("Nichrome Status : %d\r\n",nich_status);
    
    top_detect = 0x04 & (bitshift_sum);
    pc.printf("Top Detect      : %d\r\n",top_detect);
    
    save_data_status = 0x08 & (bitshift_sum);
    pc.printf("Save Data Status: %d\r\n",save_data_status);
    
    eeprom_number = im920.data[bit];
    bit += 1;
    pc.printf("eeprom_number   : %d\r\n",eeprom_number);
    
    lat = (float)im920.toFloat(bit);
    bit += 4;
    pc.printf("Latitude        : %.7f\r\n",lat);
    lon = (float)im920.toFloat(bit);
    bit += 4;
    pc.printf("Longitude       : %.7f\r\n",lon);
    /*
    height = (float)im920.toFloat(bit);
    bit += 4;
    pc.printf("Height          : %.7f\r\n",height);
    */
    press = (float)im920.toFloat(bit);
    bit += 4;
    pc.printf("Pressure        : %.2f [hPa]\r\n", press);
    temp = (short)im920.toShort(bit);
    temp /= TEMP_MULTIPLIER;
    bit += 2;
    pc.printf("Temperarure     : %.2f [degC]\r\n", temp);
    lpf_altitude = (short)im920.toShort(bit);
    lpf_altitude /= LPF_ALT_MULTIPLIER;
    bit += 2;
    pc.printf("LPF Altitude    : %.2f [m]\r\n", lpf_altitude);
    
    lpf_velocity = (short)im920.toShort(bit);
    lpf_velocity /= LPF_VEL_MULTIPLIER;
    bit += 2;
    pc.printf("Lpf Velocity    : %f [m/s]\r\n", lpf_velocity);
    
    voltage_main = (short)im920.toShort(bit);
    voltage_main /= 1000;
    bit += 2;
    pc.printf("Voltage Main    : %.2f [V]\r\n",voltage_main);
    current_main = (short)im920.toShort(bit);
    current_main /= 1000;
    bit += 2;
    pc.printf("Crrent Main     : %.2f [A]\r\n",current_main);
    voltage_sep = (short)im920.toShort(bit);
    voltage_sep /= 1000;
    bit += 2;
    pc.printf("Voltage Sep     : %.2f [V]\r\n",voltage_sep);
    current_sep = (short)im920.toShort(bit);
    current_sep /= 1000;
    bit += 4;
    pc.printf("Current Sep     : %.2f [A]\r\n",current_sep);
    
    for(int i = 0; i < 20; i++){
        pc.printf("*");
    }
    pc.printf("\r\n\r\n");
}

void ReadLaunchTime(){
    int bit = 1;
    launch_time = (float)im920.toFloat(bit);
    bit += 4;
    launch_time /= 1000;
    time_reset_counter_launch = (short)im920.toShort(bit);
    bit += 2;
    launch_minute = launch_time / 60;
    launch_second = (int)launch_time % 60;
    pc.printf("Get launch time\r\n");
    pc.printf("launch time : %f\r\n",launch_time);
}

void Read0xA5(){
    for(int i = 0; i < 20; i++){
        pc.printf("*");
    }
    pc.printf("\r\nIM920 ECHO : ");
    
    switch(im920.data[1]){
        case 'U':
        pc.printf("MODE SETUP\r\n");
        break;
        
        case 'O':
        pc.printf("MODE ONCALL\r\n");
        break;
        
        case 'F':
        pc.printf("MODE FLIGHT\r\n");
        break;
        
        case 'S':
        pc.printf("MODE SEPARATE\r\n");
        break;
        
        case 'D':
        pc.printf("MODE DESCEND\r\n");
        break;
        
        case 'C':
        pc.printf("MODE COLLECTION\r\n");
        break;

        case 0x01:
        pc.printf("SEPARATE\r\n");
        break;
        
        case 0x00:
        pc.printf("STOP SEPARATE\r\n");
        break;
    }
    
    for(int i = 0; i < 20; i++){
        pc.printf("*");
    }
    pc.printf("\r\n");
}

void Help(){
    pc.printf("\r\n");
    pc.printf("**I can help you!***\r\n");
    pc.printf("U : Mode Setup\r\n");
    pc.printf("O : Mode Oncall\r\n");
    pc.printf("F : Mode Flight\r\n");
    pc.printf("S : Mode Separate\r\n");
    pc.printf("D : Mode Descend\r\n");
    pc.printf("C : Mode Collection\r\n");
    pc.printf("0 : Stop Separate\r\n");
    pc.printf("1 : Separate\r\n");
    pc.printf("? : Help\r\n");
    pc.printf("********************\r\n\r\n");    
}