File content as of revision 0:d99384e36f64:

#include "mbed.h"
#include "ES920LR.hpp"

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

ES920LR es920(serial_es920, pc, 115200);

/*******************************************************************************
関数のプロトタイプ宣言
*******************************************************************************/
void readPC();
void printHelp();

void sendUpLink(char command);
void readHeaderSensorSetup();
void readHeaderGPSSetup();
void readHeaderData();
void readHeaderStart();

/*******************************************************************************
変数の宣言
*******************************************************************************/
int time_read, time_reset, time_min, time_sec;
int flight_time_read, flight_time_reset, flight_time_min, flight_time_sec;

char rocket_phase;

bool ex_power;
bool separate1, separate2;
bool ok_up, ok_down, ok_top;
bool save;
bool flight_pin;

float gps_lat, gps_lon, gps_alt, gps_knot, gps_deg;
int gps_sat;

float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;

float pres33, alt33, speed33;

float temp35, hum35;

float pitot_speed;

int gps_wait_count;


/*******************************************************************************
定数
*******************************************************************************/
const float ES920_MAX_20 = 0.000305176;
const float ES920_MAX_100 = 0.001525879;
const float ES920_MAX_200 = 0.003051758;
const float ES920_MAX_500 = 0.007629395;
const float ES920_MAX_1500 = 0.022888184;
const float ES920_MAX_3000 = 0.045776367;


/*******************************************************************************
無線のヘッダまとめ(ROCKET -> GND)
*******************************************************************************/
const char HEADER_SENSOR_SETUP = 0x01;
// 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
//   0     1      1       1     1     1      1     1    1    1    1
//         0      1       2     3     4      5     6    7    8    9

const char HEADER_GPS_SETUP = 0x02;
// 0x02, readable, wait_count
//   0      1          4
//          0          1
//        0x00 : NG
//        0x01 : OK
//        0xAA : Finish
//        0xFF : Ignore

const char HEADER_DATA = 0x10;
//起動時間, フライト時間, フェーズ, ex_pow&Sep1&Sep2&ok_up&ok_down&ok_top&gps_yn&save&flight_pin, lat, lon, alt, knot, deg, sat, in_v, in_i, ex_v, ex_i, pres33, alt33, speed33, temp35, hum35, pitot_speed 
//4(2,2)   4(2,2)      1         1                                                            2     2    2    2    2    2    2     2     2     2      2       2      2        2       2      2 
//0 2      4 6         8         9                                                            10    12   14   16   18   20   22    24    26    28     30      32     34       36      38     40 
//                                  0x01 : ex_pow
//                                  0x02 : Sep1
//                                  0x04 : Sep2
//                                  0x08 : ok_up
//                                  0x10 : ok_down
//                                  0x20 : ok_top
//                                  0x40 : save
//                                  0x80 : gps_yn

const char HEADER_START = 0x11;
// 0x11, What
//   0     1
//         0
//      'W' : 安全モード->離床検知モード
//      'S' : 離床検知モード->安全モード
//      'F' : 離床検知モード->フライトモード
//      '1' : 1段目分離
//      '2' : 2段目分離


/*******************************************************************************
無線のヘッダまとめ(GND -> ROCKET)
*******************************************************************************/
const char HEADER_COMMAND = 0xcd;
// 0xcd,コマンド
//   0     1
//   0     1


/*******************************************************************************
ロケットのフェーズ
*******************************************************************************/
const char ROCKET_SETUP = 0x00;     //セットアップ中
const char ROCKET_SAFETY = 0x01;    //安全モード
const char ROCKET_WAIT = 0x02;      //待機モード
const char ROCKET_FLIGHT = 0x03;    //フライトモード
const char ROCKET_SEP1 = 0x04;      //分離1モード
const char ROCKET_SEP2 = 0x05;      //分離2モード
const char ROCKET_RECOVERY = 0x06;  //回収モード


/*******************************************************************************
メイン関数
*******************************************************************************/
int main(){
    printHelp();
    
    pc.attach(&readPC, Serial::RxIrq);
    
    es920.attach(&readHeaderSensorSetup, HEADER_SENSOR_SETUP);
    es920.attach(&readHeaderGPSSetup, HEADER_GPS_SETUP);
    es920.attach(&readHeaderData, HEADER_DATA);
    es920.attach(&readHeaderStart, HEADER_START);
    
    while(1){
    }
}


/*******************************************************************************
PCからの読み取り
*******************************************************************************/
void readPC(){
    char ch = pc.getc();
    pc.printf("Input : %c\r\n", ch);
    switch(ch){
        case 'W':
        case 'S':
        case 'F':
        case '1':
        case '2':
        case 'C':
        case 'B':
        case 'O':
        sendUpLink(ch);
        break;
        
        case '?':
        printHelp();
        break;
    }
}


/*******************************************************************************
ヘルプを表示
*******************************************************************************/
void printHelp(){
    for(int i = 0; i < 20; i ++){
        pc.printf("*");
    }
    
    pc.printf("\r\nCommands\r\n");
    pc.printf("  W      : Safety -> Wait\r\n");
    pc.printf("  S      : Wait -> Safety\r\n");
    pc.printf("  F      : Wait -> Flight\r\n");
    pc.printf("  1(one) : 1st Separate 5[s]\r\n");
    pc.printf("  2(two) : 2nd Separate 5[s]\r\n");
    pc.printf("  C      : Stop Recording\r\n");
    pc.printf("  O      : Start Recording\r\n");
    pc.printf("  B      : Buzzer On/Off\r\n");
    pc.printf("  ?      : HELP\r\n");
    
    for(int i = 0; i < 20; i ++){
        pc.printf("*");
    }
    pc.printf("\r\n");
    wait(1.0f);
}


/*******************************************************************************
アップリンクを送信する
*******************************************************************************/
void sendUpLink(char command){
    es920 << HEADER_COMMAND;
    es920 << command;
    es920.send();
}


/*******************************************************************************
HEADER_SENSOR_SETUPを受信したら呼び出される
*******************************************************************************/
void readHeaderSensorSetup(){
    pc.printf("\r\n**************************************************\r\n");
    pc.printf("Sensor Setting Start!!\r\n");
    
    ///////////////////////////////////////////ADXL375
    if(es920.data[0] == 0x01){
        pc.printf("ADXL375 : OK\r\n");
    }
    else{
        pc.printf("ADXL375 : NG...\r\n");
    }
    
    ///////////////////////////////////////////INA226_in
    if(es920.data[1] == 0x01){
        pc.printf("INA226_in : OK!!\r\n");
    }
    else{
        pc.printf("INA226_in : NG...\r\n");
    }
    
    ///////////////////////////////////////////INA226_ex
    if(es920.data[2] == 0x01){
        pc.printf("INA226_ex OK!!\r\n");
    }
    else{
        pc.printf("INA226_ex NG...\r\n");
    }

    ///////////////////////////////////////////MPU9250
    if(es920.data[3] == 0x01){
        pc.printf("MPU9250 : OK!!\r\n");
    }
    else{
        pc.printf("MPU9250 : NG...\r\n");
    }
    if(es920.data[4] == 0x01){
        pc.printf("MPU9250 MAG : OK!!\r\n");
    }
    else{
        pc.printf("MPU9250 MAG : NG...\r\n");
    }
    
    ///////////////////////////////////////////LPS22HB
    if(es920.data[5] == 0x01){
        pc.printf("LPS22HB : OK!!\r\n");
    }
    else{
        pc.printf("LPS22HB : NG...\r\n");
    }
    
    ///////////////////////////////////////////LPS33HW
    if(es920.data[6] == 0x01){
        pc.printf("LPS33HW : OK!!\r\n");
    }
    else{
        pc.printf("LPS33HW : NG...\r\n");
    }
    
    ///////////////////////////////////////////SHT35
    if(es920.data[7] == 0x01){
        pc.printf("SHT35 : OK!!\r\n");
    }
    else{
        pc.printf("SHT35 : NG...\r\n");
    }
    
    ///////////////////////////////////////////TSL2561
    if(es920.data[8] == 0x01){
        pc.printf("TSL2561 : OK!!\r\n");
    }
    else{
        pc.printf("SL2561 : NG...\r\n");
    }
    
    ///////////////////////////////////////////SD
    if(es920.data[9] == 0x01){
        pc.printf("SD : OK!!\r\n");
    }
    else{
        pc.printf("SD : NG...\r\n");
    }
    
    pc.printf("Sensor Setting Finished!!\r\n");
    pc.printf("**************************************************\r\n");
}


/*******************************************************************************
HEADER_GPS_SETUPを受信したら呼び出される
*******************************************************************************/
void readHeaderGPSSetup(){
    switch(es920.data[0]){
        case 0x00:
        gps_wait_count = es920.toInt(1);
        pc.printf("GPS Wait : %d\r\n", gps_wait_count);
        break;
        
        case 0x01:
        pc.printf("GPS Wait Complete!!\r\n");
        break;
        
        case 0xAA:
        pc.printf("GPS Setting Finished!!\r\n");
        pc.printf("\r\n**************************************************\r\n");
        break;
        
        case 0xFF:
        pc.printf("GPS Setting Ignore...\r\n");
        pc.printf("\r\n**************************************************\r\n");
        break;
    }
}


/*******************************************************************************
HEADER_DATAを受信したら呼び出される
*******************************************************************************/
void readHeaderData(){
    int i = 0;                              //0
    time_reset = (int)es920.toShort(i);
    i += 2;                                 //2
    time_read = (int)es920.toShort(i);
    i += 2;                                 //4
    flight_time_reset = (int)es920.toShort(i);
    i += 2;                                 //6
    flight_time_read = (int)es920.toShort(i);
    i += 2;                                 //8
    
    rocket_phase = es920.data[i];
    i += 1;                                 //9
    
    char rocket_status = es920.data[i];
    i += 1;                                 //10
    ex_power = false;
    separate1 = false;
    separate2 = false;
    ok_up = false;
    ok_down = false;
    ok_top = false;
    save = false;
    flight_pin = false;
    if(rocket_status & 0x01)ex_power = true;
    if(rocket_status & 0x02)separate1 = true;
    if(rocket_status & 0x04)separate2 = true;
    if(rocket_status & 0x08)ok_up = true;
    if(rocket_status & 0x10)ok_down = true;
    if(rocket_status & 0x20)ok_top = true;
    if(rocket_status & 0x40)save = true;
    if(rocket_status & 0x80)flight_pin = true;
    
    gps_lat = (float)es920.toShort(i) * ES920_MAX_100;
    i += 2;                                         //12
    gps_lon = (float)es920.toShort(i) * ES920_MAX_500;
    i += 2;                                         //14
    gps_alt = (float)es920.toShort(i) * ES920_MAX_500;
    i += 2;                                         //16
    gps_knot = (float)es920.toShort(i) * ES920_MAX_200;
    i += 2;                                         //18
    gps_deg = (float)es920.toShort(i) * ES920_MAX_1500;
    i += 2;                                         //20
    gps_sat = (int)es920.toShort(i);
    i += 2;                                         //22
    ina_in_v = (float)es920.toShort(i) * ES920_MAX_20;
    i += 2;                                         //24
    ina_in_i = (float)es920.toShort(i) * ES920_MAX_20;
    i += 2;                                         //26
    ina_ex_v = (float)es920.toShort(i) * ES920_MAX_20;
    i += 2;                                         //28
    ina_ex_i = (float)es920.toShort(i) * ES920_MAX_20;
    i += 2;                                         //30
    pres33 = (float)es920.toShort(i) * ES920_MAX_3000;
    i += 2;                                         //32
    alt33 = (float)es920.toShort(i) * ES920_MAX_500;
    i += 2;                                         //34
    speed33 = (float)es920.toShort(i) * ES920_MAX_100;
    i += 2;                                         //36
    temp35 = (float)es920.toShort(i) * ES920_MAX_100;
    i += 2;                                         //38
    hum35 = (float)es920.toShort(i) * ES920_MAX_200;
    i += 2;                                         //40
    pitot_speed = (float)es920.toShort(i) * ES920_MAX_200;
    
    
    time_min = time_reset * 30 + (float)floor((double)time_read / 60);
    time_sec = time_read % 60;
    flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / 60);
    flight_time_sec = flight_time_read % 60;
    
    pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
    pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
    
    pc.printf("Phase : ");
    switch(rocket_phase){
        case ROCKET_SETUP:
        pc.printf("SETUP\r\n");
        break;
        case ROCKET_SAFETY:
        pc.printf("SAFETY\r\n");
        break;
        case ROCKET_WAIT:
        pc.printf("WAIT\r\n");
        break;
        case ROCKET_FLIGHT:
        pc.printf("FLIGHT\r\n");
        break;
        case ROCKET_SEP1:
        pc.printf("SEPARATE 1\r\n");
        break;
        case ROCKET_SEP2:
        pc.printf("SEPARATE 2\r\n");
        break;
        case ROCKET_RECOVERY:
        pc.printf("RECOVERY\r\n");
        break;
    }
    
    pc.printf("Power : ");
    if(ex_power){
        pc.printf("External\r\n");
    }
    else{
        pc.printf("Battery\r\n");
    }
    
    pc.printf("Separate : %d, %d\r\n", separate1, separate2);
    pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
    pc.printf("SAVE : %d\r\n", save);
    pc.printf("Flight Pin : %d\r\n", flight_pin);
    
    pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
    pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
    pc.printf("Sats : %d\r\n", gps_sat);
    
    pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
    pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
    
    pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
    pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
    pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
    
    pc.printf("\n\n\n");
}


/*******************************************************************************
HEADER_STARTを受信したら呼び出される
*******************************************************************************/
void readHeaderStart(){
    char what = es920.data[0];
    switch(what){
        case 'W':
        pc.printf("Phase Change : WAIT\r\n");
        break;
        
        case 'S':
        pc.printf("Phase Change : SAFETY\r\n");
        break;
        
        case 'F':
        pc.printf("Phase Change : FLIGHT\r\n");
        break;
        
        case '1':
        pc.printf("Separate1 Start\r\n");
        break;
        
        case '2':
        pc.printf("Separate2 Start\r\n");
        break;
        
        case 'C':
        pc.printf("File Closed\r\n");
        break;
        
        case 'B':
        pc.printf("Buzzer Change\r\n");
        break;
        
        default:
        pc.printf("????? Start\r\n");
        break;
    }
}