高高度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
main.cpp
- Committer:
- Sigma884
- Date:
- 2019-02-19
- Revision:
- 1:6dea30c8b406
- Parent:
- 0:03be138291de
- Child:
- 2:a443df6115bc
File content as of revision 1:6dea30c8b406:
#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; /******************************************************************************* コンストラクタ *******************************************************************************/ 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; 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; /******************************************************************************* 定数 *******************************************************************************/ /******************************************************************************* 無線のヘッダまとめ(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(){ 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; } /******************************************************************************* 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(){ } /******************************************************************************* 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; } 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); }