高高度CanSat Space SWANの投下時に使用したプログラム.(IZU2019)
Dependencies: Nichrome_lib mbed ads1115_test BNO055_lib ADXL375_i2c ES920LR CCS811_lib SDFileSystem BME280_lib INA226_lib EEPROM_lib GPS_interrupt
Diff: main.cpp
- Revision:
- 0:03be138291de
- Child:
- 1:6dea30c8b406
diff -r 000000000000 -r 03be138291de main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Feb 19 15:00:39 2019 +0000 @@ -0,0 +1,436 @@ +#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" + + +/******************************************************************************* +設定値 +投下前に必ず確認!! +*******************************************************************************/ + + +/******************************************************************************* +コンストラクタ +*******************************************************************************/ +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 readSensor(); +void readGPS(); + +void printData(); +void readPC(); +void printHelp(); + +void sendDownLink();//無線あり +void readUpLink(); //無線あり + +void startRecSlow(); +void startRecFast(); +void stopRec(); +void recData(); + + +/******************************************************************************* +変数の宣言 +*******************************************************************************/ +char CanSat_phase; +bool do_first = 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; + +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; + + +/******************************************************************************* +定数 +*******************************************************************************/ + + +/******************************************************************************* +無線のヘッダまとめ(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(); //状態遷移の判断 + } +} + + +/******************************************************************************* +状態遷移の判断 +無線あり:HEADER_START +*******************************************************************************/ +void modeChange(); + + +/******************************************************************************* +センサー読み取り +*******************************************************************************/ +void readSensor(); + + +/******************************************************************************* +GPS読み取り +*******************************************************************************/ +void readGPS(); + + +/******************************************************************************* +データ表示 +*******************************************************************************/ +void printData(); + + +/******************************************************************************* +PC読み取り +*******************************************************************************/ +void readPC(); + + +/******************************************************************************* +ヘルプ表示 +*******************************************************************************/ +void printHelp(); + + +/******************************************************************************* +ダウンリンク送信 +無線あり:HEADER_DATA +*******************************************************************************/ +void sendDownLink(); + + +/******************************************************************************* +アップリンク受信 +無線あり:HEADER_COMMAND +*******************************************************************************/ +void readUpLink(); + + +/******************************************************************************* +データを1秒ごとに書き込み開始 +*******************************************************************************/ +void startRecSlow(); + + +/******************************************************************************* +データを最速で書き込み開始 +*******************************************************************************/ +void startRecFast(); + + +/******************************************************************************* +データ記録停止 +*******************************************************************************/ +void stopRec(); + + +/******************************************************************************* +データの記録 +*******************************************************************************/ +void recData(); + + +/******************************************************************************* +セットアップ(最初に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; + } + + //////////////////////////////////////////////////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); +}