File content as of revision 2:a443df6115bc:

#include "mbed.h"
#include "math.h"

#include "Adafruit_ADS1015.h"
#include "ADXL375_i2c.h"
#include "BME280_lib.h"
#include "BNO055_lib.h"
#include "CCS811_lib.h"
#include "EEPROM_lib.h"
#include "ES920LR.hpp"
#include "GPS_interrupt.h"
#include "INA226.h"
#include "Nichrome_lib.h"
#include "SDFileSystem.h"


/*******************************************************************************
設定値
投下前に必ず確認！！
*******************************************************************************/
bool wait_GPS = true;
bool ok_PC = false;

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

RawSerial serial_es920(p9, p10);
ES920LR es920(serial_es920, pc, 115200);

Serial GPS_serial(p13, p14, 38400);
GPS_interrupt GPS(&GPS_serial);
float GPS_freq = 4;

SDFileSystem sd(p5, p6, p7, p8, "sd");
const char file_name[64] = "/sd/Space_SWAN_LOG.txt";

I2C i2c_bus(p28, p27);
ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH);
myINA226 INA226(i2c_bus, myINA226::A1_VDD, myINA226::A0_VDD);
BME280_lib BME280(i2c_bus, BME280_lib::AD0_LOW);
BNO055_lib BNO055(i2c_bus, BNO055_lib::AD0_HIGH);
CCS811_lib CCS811(i2c_bus, CCS811_lib::AD0_LOW);
EEPROM_lib EEPROM(i2c_bus, 4);

Adafruit_ADS1115 ADS1115(&i2c_bus, ADS1015_ADDRESS);

DigitalOut Valve1(p23);
DigitalOut Valve2(p24);
DigitalOut Valve3(p24);
DigitalOut Valve4(p25);
DigitalOut Buzzer(p22);

DigitalIn FlightPin(p15);

Timeout timeout_stop;
Timeout timeout_sep;
Timeout timeout_recovery;

Timer time_main;
Timer time_flight;

Ticker tick_gps;
Ticker tick_print;
Ticker tick_header_data;


/*******************************************************************************
関数のプロトタイプ宣言
*******************************************************************************/
void setup();   //無線あり

void modeChange();  //無線あり

void controllAttitude();

void readSensor();
void readGPS();

void printData();
void readPC();
void printHelp();

void sendDownLink();//無線あり
void readUpLink();  //無線あり

void startRecSlow();
void startRecFast();
void stopRec();
void recData();

void buzzerChange();


/*******************************************************************************
変数の宣言
*******************************************************************************/
char CanSat_phase;
bool do_first = false;
bool controll_yn = false;

bool valve1 = false, valve2 = false, valve3 = false, valve4 = false;
bool buzzer = false;

bool es920_using = false;
char es920_uplink_command = '-';

float adxl_acc[3];

float ina_v, ina_i;

double amg[9], quart[4], euler[3];

float pres, temp, hum, alt, pres_0, temp_0;
float pres_now;
int speed_time_old;
float alt_buf[10], alt_ave, alt_ave_old, speed, speed_buf[10], speed_ave;
int alt_count = 0, speed_count = 0;

int CO2, TVOCs;

int16_t bottle_pres_bit;
float bottle_pres;

float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
int gps_sat;
bool gps_yn;

bool flight_pin;

FILE *fp;
bool save_slow = false;
bool save_fast = false;
int save_c = 0;

int time_read, time_reset = 0;
int time_min, time_sec, time_ms;

int flight_time_read, flight_time_read_old, flight_time_reset = 0;
int flight_time_min, flight_time_sec, flight_time_ms;


/*******************************************************************************
定数
*******************************************************************************/
const float ES920_MAX_2 = 0.0000305180;
const float ES920_MAX_5 = 0.0000762951;
const float ES920_MAX_20 = 0.0003051804;
const float ES920_MAX_50 = 0.0007629511;
const float ES920_MAX_100 = 0.0015259022;
const float ES920_MAX_200 = 0.0030518044;
const float ES920_MAX_500 = 0.0076295109;
const float ES920_MAX_1500 = 0.0228885328;


/*******************************************************************************
無線のヘッダまとめ(CANSAT -> GND)
*******************************************************************************/
const char HEADER_SENSOR_SETUP = 0x01;
// 0x01, ADXL, INA, BME, BNO, CCS, PRES, SD
// 0     1     1    1    1    1    1     1
//       0     1    2    3    4    5     6

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;

const char HEADER_START = 0x11;


/*******************************************************************************
無線のヘッダまとめ(GND -> CANSAT)
*******************************************************************************/
const char HEADER_COMMAND = 0xcd;
// 0xcd, command
// 0     1
//       0


/*******************************************************************************
CanSatのフェーズ
*******************************************************************************/
const char CANSAT_SETUP = 0x00;     //セットアップ中
const char CANSAT_SAFETY = 0x01;    //安全モード
const char CANSAT_WAIT = 0x02;      //待機モード
const char CANSAT_FLIGHT = 0x03;    //フライトモード
const char CANSAT_RECOVERY = 0x04;  //回収モード


/*******************************************************************************
メイン関数
*******************************************************************************/
int main() {
    CanSat_phase = CANSAT_SETUP;
    setup();
    
    pc.attach(&readPC, Serial::RxIrq);
    
    tick_gps.attach(&readGPS, 1.0/GPS_freq);
    tick_print.attach(&printData, 1.0f);
    tick_header_data.attach(&sendDownLink, 1.0f);
    
    es920.attach(&readUpLink, HEADER_COMMAND);
    
    FlightPin.mode(PullUp);
    
    time_main.reset();
    time_main.start();
    startRecSlow();
    
    CanSat_phase = CANSAT_SAFETY;
    while(1) {
        readSensor();
        recData();
        modeChange();   //状態遷移の判断
        controllAttitude();
    }
}


/*******************************************************************************
状態遷移の判断
無線あり：HEADER_START
*******************************************************************************/
void modeChange(){
    switch(CanSat_phase){
        case CANSAT_SAFETY:
        if(!do_first){
            
        }
        break;
        
        case CANSAT_WAIT:
        if(!do_first){
            
        }
        break;
        
        case CANSAT_FLIGHT:
        if(!do_first){
            
        }
        break;
        
        case CANSAT_RECOVERY:
        if(!do_first){
            
        }
        break;
    }
}

/*******************************************************************************
姿勢制御
*******************************************************************************/
void controllAttitude(){
    
}


/*******************************************************************************
センサー読み取り
*******************************************************************************/
void readSensor(){
    time_read = time_main.read_ms();
    time_min = time_reset * 30 + floor((double)time_read / (60 * 1000));
    time_sec = time_read % (60 * 1000);
    time_sec = floor((double)time_sec / 1000);
    time_ms = time_read % 1000;
    if(time_read >= 30*60*1000){
        time_main.reset();
        time_reset ++;
    }
    
    if(CanSat_phase >= CANSAT_FLIGHT){
        flight_time_read_old = flight_time_read;
        flight_time_read = time_flight.read_ms();
    }
    else{
        flight_time_read = 0;
    }
    flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000));
    flight_time_sec = flight_time_read % (60 * 1000);
    flight_time_sec = floor((double)flight_time_sec / 1000);
    flight_time_ms = flight_time_read % 1000;
    if(time_read >= 30*60*1000){
        time_flight.reset();
        flight_time_reset ++;
    }
    
    ADXL375.getOutput(adxl_acc);
    
    INA226.get_Voltage_current(&ina_v, &ina_i);
    ina_v = ina_v / 1000;
    ina_i = ina_i / 1000;
    if(ina_i > 64){
        ina_i = 0.0f;
    }
    
    BME280.getData(&temp, &pres_now, &hum);
    if(pres_now != pres){
        pres = pres_now;
        alt = BME280.getAlt2(pres_0, temp_0);
        
        alt_buf[alt_count] = alt;
        alt_count ++;
        if(alt_count > 10){
            alt_count = 0;
        }
        float alt_sum = 0;
        for(int i = 0; i < 10; i ++){
            alt_sum += alt_buf[i];
        }
        alt_ave_old = alt_ave;
        alt_ave = alt_sum / 10;
        
        if(fabs(alt_ave - alt_ave_old) > 0.01){
            speed = (alt_ave - alt_ave_old) / (((float)flight_time_read - (float)speed_time_old) / 1000);
        }
        if(CanSat_phase <= CANSAT_WAIT){
            speed = 0.0f;
        }
        
        speed_buf[speed_count] = speed;
        speed_count ++;
        if(speed_count > 10){
            speed_count = 0;
        }
        float speed_sum = 0;
        for(int i = 0; i < 10; i ++){
            speed_sum = speed_buf[i];
        }
        speed_ave = speed_sum / 10;
        
        speed_time_old = flight_time_read;
    }
    
    if(CanSat_phase <= CANSAT_WAIT){
        pres_0 = pres;
        temp_0 = temp;
    }
    
    BNO055.getAMG(amg);
    BNO055.getQuart(quart);
    BNO055.getEulerFromQ(euler);
    
    CCS811.getData(&CO2, &TVOCs);
    
    bottle_pres_bit = ADS1115.readADC_SingleEnded(0);
    bottle_pres_bit = (float)bottle_pres_bit * 0.125f * 3.0f / 2.0f;
    bottle_pres = (float)bottle_pres_bit/1000.0f / 4.0f - 0.25f;
    
    flight_pin = FlightPin;
}


/*******************************************************************************
GPS読み取り
*******************************************************************************/
void readGPS(){
    gps_yn = GPS.gps_readable;
    if(gps_yn){
        gps_lat = GPS.Latitude();
        gps_lon = GPS.Longitude();
        gps_alt = GPS.Height();
        GPS.getUTC(gps_utc);
        gps_utc[3] += 9;
        if(gps_utc[3] >= 24){
            gps_utc[3] -= 24;
            gps_utc[2] += 1;
        }
        gps_knot = GPS.Knot();
        gps_deg = GPS.Degree();
        gps_sat = GPS.Number();
    }
}


/*******************************************************************************
データ表示
*******************************************************************************/
void printData(){
    if(ok_PC){
        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(CanSat_phase){
            case CANSAT_SETUP:
            pc.printf("SETUP\r\n");
            break;
            case CANSAT_SAFETY:
            pc.printf("SAFETY\r\n");
            break;
            case CANSAT_WAIT:
            pc.printf("WAIT\r\n");
            break;
            case CANSAT_FLIGHT:
            pc.printf("FLIGHT\r\n");
            break;
            case CANSAT_RECOVERY:
            pc.printf("RECOVERY\r\n");
            break;
        }
        
        pc.printf("SAVE : ");
        if(save_slow){
            pc.printf("SLOW\r\n");
        }
        else if(save_fast){
            pc.printf("FAST\r\n");
        }
        else{
            pc.printf("STOP\r\n");
        }
        
        pc.printf("Controll : %d\r\n", controll_yn);
        
        pc.printf("Flight Pin : %d\r\n", flight_pin);
        pc.printf("Valve: %d, %d, %d, %d\r\n", valve1, valve2, valve3, valve4);
        pc.printf("Buzzer : %d\r\n", buzzer); 
        
        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("INA226 : %.2f[V], %.2f[A]\r\n", ina_v, ina_i);
        pc.printf("BME280 : %.2f[hPa], %.2f[degC], %.2f[%%], %.2f[m], %.2f[m/s]\r\n", pres, temp, hum, alt_ave, speed_ave);
        pc.printf("BNO055 : %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
        pc.printf("CCS811 : %d[ppm], %d[ppm]\r\n", CO2, TVOCs);
        pc.printf("Bottle : %f[MPa]\r\n", bottle_pres);
        
        pc.printf("\n\n\n");
    }
}


/*******************************************************************************
PC読み取り
*******************************************************************************/
void readPC(){
    if(ok_PC){
        char command = pc.getc();
        pc.printf("PC Command : %c\r\n", command);
        
        switch(command){
            case '?':   //ヘルプ表示
            printHelp();
            break;
            
            case 'W':   //投下待機モードへ移行
            if(CanSat_phase == CANSAT_SAFETY){
                CanSat_phase = CANSAT_WAIT;
                do_first = false;
            }
            break;
            
            case 'S':   //安全モードへ移行
            if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){
                CanSat_phase = CANSAT_SAFETY;
                do_first = false;
            }
            break;
            
            case 'F':   //フライトモードへ移行
            if(CanSat_phase == CANSAT_WAIT){
                CanSat_phase = CANSAT_FLIGHT;
                do_first = false;
            }
            break;
            
            case 'C':   //記録停止
            if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){
                stopRec();
            }
            break;
            
            case 'o':   //低速記録開始
            if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
                startRecSlow();
            }
            break;
            
            case 'O':   //高速記録開始
            if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
                startRecFast();
            }
            break;
            
            case 'B':   //ブザー切り替え
            buzzerChange();
            break;
            
            case '1':   //姿勢制御再開
            if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){
                controll_yn = true;
            }
            break;
            
            case '0':   //姿勢制御停止
            if(CanSat_phase == CANSAT_FLIGHT && controll_yn){
                controll_yn = false;
            }
            break;
        }
    }
}


/*******************************************************************************
ヘルプ表示
*******************************************************************************/
void printHelp(){
    pc.printf("\r\n********************\r\n");
    pc.printf("Commands\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("C          : Stop Recording\r\n");
    pc.printf("o(small o) : Start Recording (Slow)\r\n");
    pc.printf("O(large o) : Start Recording (Fast)\r\n");
    pc.printf("B          : Buzzer Change\r\n");
    pc.printf("1(one)     : Restart Controll Attitude\r\n");
    pc.printf("0(zero)    : Stop Controll Attitude\r\n");
    
    pc.printf("********************\r\n\n");
    wait(1.0f);
}


/*******************************************************************************
ダウンリンク送信
無線あり：HEADER_DATA
*******************************************************************************/
void sendDownLink(){
    if(!es920_using){
        es920 << HEADER_DATA;
        
        es920 << (short)time_reset;
        es920 << (short)(time_read / 1000);
        
        es920 << (short)flight_time_reset;
        es920 << (short)(flight_time_read / 1000);
        
        es920 << CanSat_phase;
        
        char status = 0x00;
        if(save_slow) status |= (char)0x01;
        if(save_fast) status |= (char)0x02;
        if(controll_yn) status |= (char)0x04;
        if(flight_pin) status |= (char)0x08;
        if(Buzzer) status |= (char)0x10;
        es920 << status;
        
        if(valve1) status |= (char)0x01;
        if(valve2) status |= (char)0x02;
        if(valve3) status |= (char)0x04;
        if(valve4) status |= (char)0x08;
        es920 << status;
        
        es920 << (float)gps_lat;
        es920 << (float)gps_lon;
        es920 << (unsigned short)(gps_alt / ES920_MAX_1500);
        es920 << (unsigned short)(gps_knot / ES920_MAX_200);
        es920 << (unsigned short)(gps_deg / ES920_MAX_500);
        es920 << (short)gps_sat;
        
        es920 << (unsigned short)(ina_v / ES920_MAX_20);
        es920 << (unsigned short)(ina_i / ES920_MAX_20);
        
        es920 << (unsigned short)(pres / ES920_MAX_1500);
        es920 << (unsigned short)(temp / ES920_MAX_100);
        es920 << (unsigned short)(alt / ES920_MAX_500);
        es920 << (unsigned short)(speed / ES920_MAX_50);
        
        es920 << (short)(euler[0] / ES920_MAX_5);
        es920 << (short)(euler[1] / ES920_MAX_5);
        es920 << (short)(euler[2] / ES920_MAX_5);
        
        es920 << (unsigned short)CO2;
        es920 << (unsigned short)TVOCs;
        
        es920 << (unsigned short)(bottle_pres / ES920_MAX_2);
        
        es920.send();
    }
}


/*******************************************************************************
アップリンク受信
無線あり：HEADER_COMMAND
*******************************************************************************/
void readUpLink(){
    
}


/*******************************************************************************
データを1秒ごとに書き込み開始
*******************************************************************************/
void startRecSlow(){
    
}


/*******************************************************************************
データを最速で書き込み開始
*******************************************************************************/
void startRecFast(){
    
}


/*******************************************************************************
データ記録停止
*******************************************************************************/
void stopRec(){
    
}


/*******************************************************************************
データの記録
*******************************************************************************/
void recData(){
    
}


/*******************************************************************************
ブザーのオンオフ切り替え
*******************************************************************************/
void buzzerChange(){
    
}


/*******************************************************************************
セットアップ（最初に1回実行）
無線あり：HEADER_SETUP_SENSOR
無線あり：HEADER_GPS_SETUP
*******************************************************************************/
void setup(){
    wait(1.0f);
    char setup_string[1024];
    
    pc.printf("\r\n******************************\r\n");
    pc.printf("Sensor Setting Start!!\r\n");
    strcat(setup_string, "Sensor Setting Start!!\r\n");
    es920 << HEADER_SENSOR_SETUP;
    
    //////////////////////////////////////////////////ADXL375
    ADXL375.setDataRate(ADXL375_100HZ);
    if(ADXL375.whoAmI() == 1){
        pc.printf("ADXL375 : OK\r\n");
        strcat(setup_string, "ADXL375 : OK\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("ADXL375 : NG\r\n");
        strcat(setup_string, "ADXL375 : NG\r\n");
        es920 << (char)0x00;
    }
    ADXL375.offset(0.0f, 0.0f, 0.0f);
    
    //////////////////////////////////////////////////INA226
    INA226.set_callibretion();
    if(INA226.Connection_check() == 0){
        pc.printf("INA226 : OK\r\n");
        strcat(setup_string, "INA226 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("INA226 : NG\r\n");
        strcat(setup_string, "INA226 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////BME280
    BME280.configMeasure(BME280_lib::NORMAL, 1, 2, 1);
    BME280.configFilter(4);
    if(BME280.connectCheck() == 1){
        pc.printf("BME280 OK!!\r\n");
        strcat(setup_string, "BME280 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("BME280 NG\r\n");
        strcat(setup_string, "BME280 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////BNO055
    BNO055.setOperationMode(BNO055_lib::CONFIG);
    BNO055.setAccRange(BNO055_lib::_16G);
    BNO055.setAxis(BNO055_lib::Z, BNO055_lib::Y, BNO055_lib::X);
    BNO055.setAxisPM(1, 1, -1);
    if(BNO055.connectCheck() == 1){
        pc.printf("BNO055 OK!!\r\n");
        strcat(setup_string, "BNO055 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("BNO055 NG\r\n");
        strcat(setup_string, "BNO055 : NG\r\n");
        es920 << (char)0x00;
    }
    BNO055.setOperationMode(BNO055_lib::NDOF);
    
    //////////////////////////////////////////////////CCS811
    if(CCS811.connectCheck() == 1){
        pc.printf("CCS811 OK!!\r\n");
        strcat(setup_string, "CCS811 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("CCS811 NG\r\n");
        strcat(setup_string, "CCS811 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////圧力センサ
    pc.printf("PRES : ");
    ADS1115.setGain(GAIN_TWOTHIRDS);
    pc.printf("OK!!\r\n");
    strcat(setup_string, "PRES : OK!!\r\n");
    es920 << (char)0x01;
    
    //////////////////////////////////////////////////SD
    fp = fopen("/sd/Space_SWAN_setup.txt", "r");
    if(fp != NULL){
        pc.printf("SD : OK!!\r\n");
        strcat(setup_string, "SD : OK!!\r\n");
        es920 << (char)0x01;
        fclose(fp);
    }
    else{
        pc.printf("SD : NG...\r\n");
        strcat(setup_string, "SD : NG...\r\n");
        es920 << (char)0x00;
    }
    
    pc.printf("Sensor Setting Finished!!\r\n");
    pc.printf("******************************\r\n");
    strcat(setup_string, "Sensor Setting Finished!!\r\n");
    es920.send();
    wait(1.0f);
    
    pc.printf("\r\n******************************\r\n");
    if(wait_GPS){
        pc.printf("GPS Setting Start!!\r\n");
        strcat(setup_string, "GPS Setting Start!!\r\n");
        
        pc.printf("Waiting : 0");
        strcat(setup_string, "Wait : ");
        int gps_wait_count = 0;
        while(!GPS.gps_readable){
            pc.printf("\rWaiting : %d", gps_wait_count);//
            es920 << HEADER_GPS_SETUP;
            es920 << (char)GPS.gps_readable;
            es920 << (int)gps_wait_count;
            es920.send();
            wait(1.0f);
            gps_wait_count ++;
        }
        char ss[8];
        sprintf(ss, "%d", gps_wait_count);
        strcat(setup_string, ss);
        pc.printf("  OK!!\r\n");
        strcat(setup_string, " OK!!\r\n");
        pc.printf("GPS Setting Finished!!\r\n");
        strcat(setup_string, "GPS Setting Finished!!\r\n");
        es920 << HEADER_GPS_SETUP;
        es920 << (char)0xAA;
        es920 << (int)0;
        es920.send();
    }
    else{
        pc.printf("GPS Setting Ignore...\r\n");
        strcat(setup_string, "GPS Setting Ignore...\r\n");
        es920 << HEADER_GPS_SETUP;
        es920 << (char)0xFF;
        es920 << (int)0;
        es920.send();
    }
    pc.printf("******************************\r\n");
    fp = fopen(file_name, "a");
    fprintf(fp, setup_string);
    fclose(fp);
    setup_string[0] = NULL;
    
    printHelp();
    wait(1.0f);
}