高高度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@6:f2016544b9e4, 2019-02-23 (annotated)
- Committer:
- Sigma884
- Date:
- Sat Feb 23 11:50:05 2019 +0000
- Revision:
- 6:f2016544b9e4
- Parent:
- 5:b71a554ab4c4
- Child:
- 7:3a2d0474f1ca
20190223 ver.0.7
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sigma884 | 0:03be138291de | 1 | #include "mbed.h" |
Sigma884 | 0:03be138291de | 2 | #include "math.h" |
Sigma884 | 0:03be138291de | 3 | |
Sigma884 | 0:03be138291de | 4 | #include "Adafruit_ADS1015.h" |
Sigma884 | 0:03be138291de | 5 | #include "ADXL375_i2c.h" |
Sigma884 | 0:03be138291de | 6 | #include "BME280_lib.h" |
Sigma884 | 0:03be138291de | 7 | #include "BNO055_lib.h" |
Sigma884 | 0:03be138291de | 8 | #include "CCS811_lib.h" |
Sigma884 | 0:03be138291de | 9 | #include "EEPROM_lib.h" |
Sigma884 | 0:03be138291de | 10 | #include "ES920LR.hpp" |
Sigma884 | 0:03be138291de | 11 | #include "GPS_interrupt.h" |
Sigma884 | 0:03be138291de | 12 | #include "INA226.h" |
Sigma884 | 0:03be138291de | 13 | #include "Nichrome_lib.h" |
Sigma884 | 0:03be138291de | 14 | #include "SDFileSystem.h" |
Sigma884 | 0:03be138291de | 15 | |
Sigma884 | 0:03be138291de | 16 | |
Sigma884 | 0:03be138291de | 17 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 18 | 設定値 |
Sigma884 | 0:03be138291de | 19 | 投下前に必ず確認!! |
Sigma884 | 0:03be138291de | 20 | *******************************************************************************/ |
Sigma884 | 5:b71a554ab4c4 | 21 | const float START_CONTROLL_TIME = 1.0f; |
Sigma884 | 5:b71a554ab4c4 | 22 | const float START_CONTROLL_ALT = -5.0f; |
Sigma884 | 5:b71a554ab4c4 | 23 | |
Sigma884 | 5:b71a554ab4c4 | 24 | const float RECOVERY_TIME = 120.0f; |
Sigma884 | 5:b71a554ab4c4 | 25 | const float RECOVERY_SPEED = -2.0f; |
Sigma884 | 5:b71a554ab4c4 | 26 | |
Sigma884 | 1:6dea30c8b406 | 27 | bool wait_GPS = true; |
Sigma884 | 2:a443df6115bc | 28 | bool ok_PC = false; |
Sigma884 | 0:03be138291de | 29 | |
Sigma884 | 0:03be138291de | 30 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 31 | コンストラクタ |
Sigma884 | 0:03be138291de | 32 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 33 | RawSerial pc(USBTX, USBRX, 115200); |
Sigma884 | 0:03be138291de | 34 | |
Sigma884 | 0:03be138291de | 35 | RawSerial serial_es920(p9, p10); |
Sigma884 | 0:03be138291de | 36 | ES920LR es920(serial_es920, pc, 115200); |
Sigma884 | 0:03be138291de | 37 | |
Sigma884 | 0:03be138291de | 38 | Serial GPS_serial(p13, p14, 38400); |
Sigma884 | 0:03be138291de | 39 | GPS_interrupt GPS(&GPS_serial); |
Sigma884 | 0:03be138291de | 40 | float GPS_freq = 4; |
Sigma884 | 0:03be138291de | 41 | |
Sigma884 | 0:03be138291de | 42 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
Sigma884 | 0:03be138291de | 43 | const char file_name[64] = "/sd/Space_SWAN_LOG.txt"; |
Sigma884 | 0:03be138291de | 44 | |
Sigma884 | 0:03be138291de | 45 | I2C i2c_bus(p28, p27); |
Sigma884 | 0:03be138291de | 46 | ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH); |
Sigma884 | 0:03be138291de | 47 | myINA226 INA226(i2c_bus, myINA226::A1_VDD, myINA226::A0_VDD); |
Sigma884 | 0:03be138291de | 48 | BME280_lib BME280(i2c_bus, BME280_lib::AD0_LOW); |
Sigma884 | 0:03be138291de | 49 | BNO055_lib BNO055(i2c_bus, BNO055_lib::AD0_HIGH); |
Sigma884 | 0:03be138291de | 50 | CCS811_lib CCS811(i2c_bus, CCS811_lib::AD0_LOW); |
Sigma884 | 0:03be138291de | 51 | EEPROM_lib EEPROM(i2c_bus, 4); |
Sigma884 | 0:03be138291de | 52 | |
Sigma884 | 0:03be138291de | 53 | Adafruit_ADS1115 ADS1115(&i2c_bus, ADS1015_ADDRESS); |
Sigma884 | 0:03be138291de | 54 | |
Sigma884 | 3:4571111a5996 | 55 | Nichrome_lib Valve1(p23); |
Sigma884 | 3:4571111a5996 | 56 | Nichrome_lib Valve2(p24); |
Sigma884 | 3:4571111a5996 | 57 | Nichrome_lib Valve3(p24); |
Sigma884 | 3:4571111a5996 | 58 | Nichrome_lib Valve4(p25); |
Sigma884 | 3:4571111a5996 | 59 | Nichrome_lib Buzzer(p22); |
Sigma884 | 0:03be138291de | 60 | |
Sigma884 | 0:03be138291de | 61 | DigitalIn FlightPin(p15); |
Sigma884 | 0:03be138291de | 62 | |
Sigma884 | 5:b71a554ab4c4 | 63 | Timeout timeout_start_controll; |
Sigma884 | 0:03be138291de | 64 | Timeout timeout_recovery; |
Sigma884 | 0:03be138291de | 65 | |
Sigma884 | 0:03be138291de | 66 | Timer time_main; |
Sigma884 | 0:03be138291de | 67 | Timer time_flight; |
Sigma884 | 0:03be138291de | 68 | |
Sigma884 | 0:03be138291de | 69 | Ticker tick_gps; |
Sigma884 | 0:03be138291de | 70 | Ticker tick_print; |
Sigma884 | 3:4571111a5996 | 71 | Ticker tick_save; |
Sigma884 | 0:03be138291de | 72 | Ticker tick_header_data; |
Sigma884 | 0:03be138291de | 73 | |
Sigma884 | 0:03be138291de | 74 | |
Sigma884 | 0:03be138291de | 75 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 76 | 関数のプロトタイプ宣言 |
Sigma884 | 0:03be138291de | 77 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 78 | void setup(); //無線あり |
Sigma884 | 0:03be138291de | 79 | |
Sigma884 | 0:03be138291de | 80 | void modeChange(); //無線あり |
Sigma884 | 5:b71a554ab4c4 | 81 | void changePhase_RECOVERY(); |
Sigma884 | 5:b71a554ab4c4 | 82 | void startControllAttitude(); |
Sigma884 | 0:03be138291de | 83 | |
Sigma884 | 2:a443df6115bc | 84 | void controllAttitude(); |
Sigma884 | 2:a443df6115bc | 85 | |
Sigma884 | 0:03be138291de | 86 | void readSensor(); |
Sigma884 | 0:03be138291de | 87 | void readGPS(); |
Sigma884 | 0:03be138291de | 88 | |
Sigma884 | 0:03be138291de | 89 | void printData(); |
Sigma884 | 0:03be138291de | 90 | void readPC(); |
Sigma884 | 0:03be138291de | 91 | void printHelp(); |
Sigma884 | 0:03be138291de | 92 | |
Sigma884 | 0:03be138291de | 93 | void sendDownLink();//無線あり |
Sigma884 | 0:03be138291de | 94 | void readUpLink(); //無線あり |
Sigma884 | 0:03be138291de | 95 | |
Sigma884 | 0:03be138291de | 96 | void startRecSlow(); |
Sigma884 | 0:03be138291de | 97 | void startRecFast(); |
Sigma884 | 0:03be138291de | 98 | void stopRec(); |
Sigma884 | 0:03be138291de | 99 | void recData(); |
Sigma884 | 0:03be138291de | 100 | |
Sigma884 | 2:a443df6115bc | 101 | void buzzerChange(); |
Sigma884 | 2:a443df6115bc | 102 | |
Sigma884 | 0:03be138291de | 103 | |
Sigma884 | 0:03be138291de | 104 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 105 | 変数の宣言 |
Sigma884 | 0:03be138291de | 106 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 107 | char CanSat_phase; |
Sigma884 | 0:03be138291de | 108 | bool do_first = false; |
Sigma884 | 2:a443df6115bc | 109 | bool controll_yn = false; |
Sigma884 | 2:a443df6115bc | 110 | |
Sigma884 | 0:03be138291de | 111 | bool es920_using = false; |
Sigma884 | 0:03be138291de | 112 | char es920_uplink_command = '-'; |
Sigma884 | 0:03be138291de | 113 | |
Sigma884 | 0:03be138291de | 114 | float adxl_acc[3]; |
Sigma884 | 0:03be138291de | 115 | |
Sigma884 | 0:03be138291de | 116 | float ina_v, ina_i; |
Sigma884 | 0:03be138291de | 117 | |
Sigma884 | 0:03be138291de | 118 | double amg[9], quart[4], euler[3]; |
Sigma884 | 0:03be138291de | 119 | |
Sigma884 | 0:03be138291de | 120 | float pres, temp, hum, alt, pres_0, temp_0; |
Sigma884 | 0:03be138291de | 121 | float pres_now; |
Sigma884 | 0:03be138291de | 122 | int speed_time_old; |
Sigma884 | 0:03be138291de | 123 | float alt_buf[10], alt_ave, alt_ave_old, speed, speed_buf[10], speed_ave; |
Sigma884 | 0:03be138291de | 124 | int alt_count = 0, speed_count = 0; |
Sigma884 | 5:b71a554ab4c4 | 125 | float alt_flight_start; |
Sigma884 | 0:03be138291de | 126 | |
Sigma884 | 1:6dea30c8b406 | 127 | int CO2, TVOCs; |
Sigma884 | 1:6dea30c8b406 | 128 | |
Sigma884 | 1:6dea30c8b406 | 129 | int16_t bottle_pres_bit; |
Sigma884 | 1:6dea30c8b406 | 130 | float bottle_pres; |
Sigma884 | 1:6dea30c8b406 | 131 | |
Sigma884 | 0:03be138291de | 132 | float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg; |
Sigma884 | 0:03be138291de | 133 | int gps_sat; |
Sigma884 | 0:03be138291de | 134 | bool gps_yn; |
Sigma884 | 0:03be138291de | 135 | |
Sigma884 | 0:03be138291de | 136 | bool flight_pin; |
Sigma884 | 0:03be138291de | 137 | |
Sigma884 | 0:03be138291de | 138 | FILE *fp; |
Sigma884 | 0:03be138291de | 139 | bool save_slow = false; |
Sigma884 | 0:03be138291de | 140 | bool save_fast = false; |
Sigma884 | 0:03be138291de | 141 | int save_c = 0; |
Sigma884 | 0:03be138291de | 142 | |
Sigma884 | 4:b878dce9c247 | 143 | int eeprom_ptr; |
Sigma884 | 4:b878dce9c247 | 144 | |
Sigma884 | 0:03be138291de | 145 | int time_read, time_reset = 0; |
Sigma884 | 0:03be138291de | 146 | int time_min, time_sec, time_ms; |
Sigma884 | 0:03be138291de | 147 | |
Sigma884 | 0:03be138291de | 148 | int flight_time_read, flight_time_read_old, flight_time_reset = 0; |
Sigma884 | 0:03be138291de | 149 | int flight_time_min, flight_time_sec, flight_time_ms; |
Sigma884 | 0:03be138291de | 150 | |
Sigma884 | 0:03be138291de | 151 | |
Sigma884 | 0:03be138291de | 152 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 153 | 定数 |
Sigma884 | 0:03be138291de | 154 | *******************************************************************************/ |
Sigma884 | 2:a443df6115bc | 155 | const float ES920_MAX_2 = 0.0000305180; |
Sigma884 | 2:a443df6115bc | 156 | const float ES920_MAX_5 = 0.0000762951; |
Sigma884 | 2:a443df6115bc | 157 | const float ES920_MAX_20 = 0.0003051804; |
Sigma884 | 2:a443df6115bc | 158 | const float ES920_MAX_50 = 0.0007629511; |
Sigma884 | 2:a443df6115bc | 159 | const float ES920_MAX_100 = 0.0015259022; |
Sigma884 | 2:a443df6115bc | 160 | const float ES920_MAX_200 = 0.0030518044; |
Sigma884 | 2:a443df6115bc | 161 | const float ES920_MAX_500 = 0.0076295109; |
Sigma884 | 2:a443df6115bc | 162 | const float ES920_MAX_1500 = 0.0228885328; |
Sigma884 | 0:03be138291de | 163 | |
Sigma884 | 0:03be138291de | 164 | |
Sigma884 | 0:03be138291de | 165 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 166 | 無線のヘッダまとめ(CANSAT -> GND) |
Sigma884 | 0:03be138291de | 167 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 168 | const char HEADER_SENSOR_SETUP = 0x01; |
Sigma884 | 0:03be138291de | 169 | // 0x01, ADXL, INA, BME, BNO, CCS, PRES, SD |
Sigma884 | 0:03be138291de | 170 | // 0 1 1 1 1 1 1 1 |
Sigma884 | 0:03be138291de | 171 | // 0 1 2 3 4 5 6 |
Sigma884 | 0:03be138291de | 172 | |
Sigma884 | 0:03be138291de | 173 | const char HEADER_GPS_SETUP = 0x02; |
Sigma884 | 0:03be138291de | 174 | // 0x02, readable, wait_count |
Sigma884 | 0:03be138291de | 175 | // 0 1 4 |
Sigma884 | 0:03be138291de | 176 | // 0 1 |
Sigma884 | 0:03be138291de | 177 | // 0x00 : NG |
Sigma884 | 0:03be138291de | 178 | // 0x01 : OK |
Sigma884 | 0:03be138291de | 179 | // 0xAA : Finish |
Sigma884 | 0:03be138291de | 180 | // 0xFF : Ignore |
Sigma884 | 0:03be138291de | 181 | |
Sigma884 | 0:03be138291de | 182 | const char HEADER_DATA = 0x10; |
Sigma884 | 6:f2016544b9e4 | 183 | // 0x10, 起動時間, フライト時間, フェーズ, status, valve, lat, lon, alt, knot, deg, sat, |
Sigma884 | 6:f2016544b9e4 | 184 | // 0 4(2,2) 4(2,2) 1 1 1 4 4 2 2 2 2 |
Sigma884 | 6:f2016544b9e4 | 185 | // 0 2 4 6 8 9 10 11 15 19 21 23 25 |
Sigma884 | 6:f2016544b9e4 | 186 | // 0x01 : save_slow |
Sigma884 | 6:f2016544b9e4 | 187 | // 0x02 : save_fast |
Sigma884 | 6:f2016544b9e4 | 188 | // 0x04 : controll_yn |
Sigma884 | 6:f2016544b9e4 | 189 | // 0x08 : flight_pin |
Sigma884 | 6:f2016544b9e4 | 190 | // 0x10 : buzzer |
Sigma884 | 6:f2016544b9e4 | 191 | |
Sigma884 | 6:f2016544b9e4 | 192 | // V, I, pres, temp, alt, speed, eulerX3, CO2, TVOCs, bottle |
Sigma884 | 6:f2016544b9e4 | 193 | // 2 2 2 2 2 2 6(2,2,2) 2 2 2 |
Sigma884 | 6:f2016544b9e4 | 194 | // 27 29 31 33 35 37 394143 45 47 49 |
Sigma884 | 0:03be138291de | 195 | |
Sigma884 | 0:03be138291de | 196 | const char HEADER_START = 0x11; |
Sigma884 | 6:f2016544b9e4 | 197 | // 0x11, What |
Sigma884 | 6:f2016544b9e4 | 198 | // 0 1 |
Sigma884 | 6:f2016544b9e4 | 199 | // 'W' : 投下検知モード |
Sigma884 | 6:f2016544b9e4 | 200 | // 'S' : 安全モード |
Sigma884 | 6:f2016544b9e4 | 201 | // 'F' : フライトモード |
Sigma884 | 6:f2016544b9e4 | 202 | // 'R' : 回収モード |
Sigma884 | 0:03be138291de | 203 | |
Sigma884 | 0:03be138291de | 204 | |
Sigma884 | 0:03be138291de | 205 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 206 | 無線のヘッダまとめ(GND -> CANSAT) |
Sigma884 | 0:03be138291de | 207 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 208 | const char HEADER_COMMAND = 0xcd; |
Sigma884 | 0:03be138291de | 209 | // 0xcd, command |
Sigma884 | 0:03be138291de | 210 | // 0 1 |
Sigma884 | 0:03be138291de | 211 | // 0 |
Sigma884 | 0:03be138291de | 212 | |
Sigma884 | 0:03be138291de | 213 | |
Sigma884 | 0:03be138291de | 214 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 215 | CanSatのフェーズ |
Sigma884 | 0:03be138291de | 216 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 217 | const char CANSAT_SETUP = 0x00; //セットアップ中 |
Sigma884 | 0:03be138291de | 218 | const char CANSAT_SAFETY = 0x01; //安全モード |
Sigma884 | 0:03be138291de | 219 | const char CANSAT_WAIT = 0x02; //待機モード |
Sigma884 | 0:03be138291de | 220 | const char CANSAT_FLIGHT = 0x03; //フライトモード |
Sigma884 | 0:03be138291de | 221 | const char CANSAT_RECOVERY = 0x04; //回収モード |
Sigma884 | 0:03be138291de | 222 | |
Sigma884 | 0:03be138291de | 223 | |
Sigma884 | 0:03be138291de | 224 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 225 | メイン関数 |
Sigma884 | 0:03be138291de | 226 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 227 | int main() { |
Sigma884 | 0:03be138291de | 228 | CanSat_phase = CANSAT_SETUP; |
Sigma884 | 0:03be138291de | 229 | setup(); |
Sigma884 | 0:03be138291de | 230 | |
Sigma884 | 0:03be138291de | 231 | pc.attach(&readPC, Serial::RxIrq); |
Sigma884 | 0:03be138291de | 232 | |
Sigma884 | 0:03be138291de | 233 | tick_gps.attach(&readGPS, 1.0/GPS_freq); |
Sigma884 | 0:03be138291de | 234 | tick_print.attach(&printData, 1.0f); |
Sigma884 | 0:03be138291de | 235 | tick_header_data.attach(&sendDownLink, 1.0f); |
Sigma884 | 0:03be138291de | 236 | |
Sigma884 | 0:03be138291de | 237 | es920.attach(&readUpLink, HEADER_COMMAND); |
Sigma884 | 0:03be138291de | 238 | |
Sigma884 | 0:03be138291de | 239 | FlightPin.mode(PullUp); |
Sigma884 | 0:03be138291de | 240 | |
Sigma884 | 0:03be138291de | 241 | time_main.reset(); |
Sigma884 | 0:03be138291de | 242 | time_main.start(); |
Sigma884 | 0:03be138291de | 243 | startRecSlow(); |
Sigma884 | 0:03be138291de | 244 | |
Sigma884 | 0:03be138291de | 245 | CanSat_phase = CANSAT_SAFETY; |
Sigma884 | 0:03be138291de | 246 | while(1) { |
Sigma884 | 0:03be138291de | 247 | readSensor(); |
Sigma884 | 0:03be138291de | 248 | modeChange(); //状態遷移の判断 |
Sigma884 | 2:a443df6115bc | 249 | controllAttitude(); |
Sigma884 | 3:4571111a5996 | 250 | if(save_fast){ |
Sigma884 | 3:4571111a5996 | 251 | recData(); |
Sigma884 | 3:4571111a5996 | 252 | } |
Sigma884 | 0:03be138291de | 253 | } |
Sigma884 | 0:03be138291de | 254 | } |
Sigma884 | 0:03be138291de | 255 | |
Sigma884 | 0:03be138291de | 256 | |
Sigma884 | 0:03be138291de | 257 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 258 | 状態遷移の判断 |
Sigma884 | 0:03be138291de | 259 | 無線あり:HEADER_START |
Sigma884 | 0:03be138291de | 260 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 261 | void modeChange(){ |
Sigma884 | 2:a443df6115bc | 262 | switch(CanSat_phase){ |
Sigma884 | 5:b71a554ab4c4 | 263 | case CANSAT_SAFETY:////////////////////安全モード |
Sigma884 | 2:a443df6115bc | 264 | if(!do_first){ |
Sigma884 | 5:b71a554ab4c4 | 265 | es920_using = true; |
Sigma884 | 5:b71a554ab4c4 | 266 | es920 << HEADER_START; |
Sigma884 | 5:b71a554ab4c4 | 267 | es920 << 'S'; |
Sigma884 | 5:b71a554ab4c4 | 268 | es920.send(); |
Sigma884 | 5:b71a554ab4c4 | 269 | es920_using = false; |
Sigma884 | 2:a443df6115bc | 270 | |
Sigma884 | 5:b71a554ab4c4 | 271 | do_first = true; |
Sigma884 | 5:b71a554ab4c4 | 272 | } |
Sigma884 | 5:b71a554ab4c4 | 273 | break; |
Sigma884 | 5:b71a554ab4c4 | 274 | |
Sigma884 | 5:b71a554ab4c4 | 275 | case CANSAT_WAIT:////////////////////待機モード |
Sigma884 | 5:b71a554ab4c4 | 276 | if(!do_first){ |
Sigma884 | 5:b71a554ab4c4 | 277 | es920_using = true; |
Sigma884 | 5:b71a554ab4c4 | 278 | es920 << HEADER_START; |
Sigma884 | 5:b71a554ab4c4 | 279 | es920 << 'W'; |
Sigma884 | 5:b71a554ab4c4 | 280 | es920.send(); |
Sigma884 | 5:b71a554ab4c4 | 281 | es920_using = false; |
Sigma884 | 5:b71a554ab4c4 | 282 | |
Sigma884 | 5:b71a554ab4c4 | 283 | do_first = true; |
Sigma884 | 5:b71a554ab4c4 | 284 | } |
Sigma884 | 5:b71a554ab4c4 | 285 | if(flight_pin){ |
Sigma884 | 5:b71a554ab4c4 | 286 | CanSat_phase = CANSAT_FLIGHT; |
Sigma884 | 5:b71a554ab4c4 | 287 | do_first = false; |
Sigma884 | 2:a443df6115bc | 288 | } |
Sigma884 | 2:a443df6115bc | 289 | break; |
Sigma884 | 2:a443df6115bc | 290 | |
Sigma884 | 5:b71a554ab4c4 | 291 | case CANSAT_FLIGHT:////////////////////フライトモード |
Sigma884 | 2:a443df6115bc | 292 | if(!do_first){ |
Sigma884 | 5:b71a554ab4c4 | 293 | es920_using = true; |
Sigma884 | 5:b71a554ab4c4 | 294 | es920 << HEADER_START; |
Sigma884 | 5:b71a554ab4c4 | 295 | es920 << 'F'; |
Sigma884 | 5:b71a554ab4c4 | 296 | es920.send(); |
Sigma884 | 5:b71a554ab4c4 | 297 | es920_using = false; |
Sigma884 | 2:a443df6115bc | 298 | |
Sigma884 | 5:b71a554ab4c4 | 299 | if(save_slow && !save_fast){ |
Sigma884 | 5:b71a554ab4c4 | 300 | stopRec(); |
Sigma884 | 5:b71a554ab4c4 | 301 | startRecFast(); |
Sigma884 | 5:b71a554ab4c4 | 302 | } |
Sigma884 | 5:b71a554ab4c4 | 303 | else if(!save_slow && !save_fast){ |
Sigma884 | 5:b71a554ab4c4 | 304 | startRecFast(); |
Sigma884 | 5:b71a554ab4c4 | 305 | } |
Sigma884 | 5:b71a554ab4c4 | 306 | |
Sigma884 | 5:b71a554ab4c4 | 307 | time_flight.reset(); |
Sigma884 | 5:b71a554ab4c4 | 308 | time_flight.start(); |
Sigma884 | 5:b71a554ab4c4 | 309 | |
Sigma884 | 5:b71a554ab4c4 | 310 | alt_flight_start = alt; |
Sigma884 | 5:b71a554ab4c4 | 311 | timeout_start_controll.attach(&startControllAttitude, START_CONTROLL_TIME); |
Sigma884 | 5:b71a554ab4c4 | 312 | |
Sigma884 | 5:b71a554ab4c4 | 313 | timeout_recovery.attach(&changePhase_RECOVERY, RECOVERY_TIME); |
Sigma884 | 5:b71a554ab4c4 | 314 | |
Sigma884 | 5:b71a554ab4c4 | 315 | do_first = true; |
Sigma884 | 2:a443df6115bc | 316 | } |
Sigma884 | 5:b71a554ab4c4 | 317 | if(alt - alt_flight_start > START_CONTROLL_ALT){ |
Sigma884 | 6:f2016544b9e4 | 318 | timeout_start_controll.detach(); |
Sigma884 | 5:b71a554ab4c4 | 319 | startControllAttitude(); |
Sigma884 | 5:b71a554ab4c4 | 320 | } |
Sigma884 | 5:b71a554ab4c4 | 321 | if(speed_ave > RECOVERY_SPEED){ |
Sigma884 | 6:f2016544b9e4 | 322 | timeout_recovery.detach(); |
Sigma884 | 5:b71a554ab4c4 | 323 | changePhase_RECOVERY(); |
Sigma884 | 2:a443df6115bc | 324 | } |
Sigma884 | 2:a443df6115bc | 325 | break; |
Sigma884 | 2:a443df6115bc | 326 | |
Sigma884 | 2:a443df6115bc | 327 | case CANSAT_RECOVERY: |
Sigma884 | 2:a443df6115bc | 328 | if(!do_first){ |
Sigma884 | 5:b71a554ab4c4 | 329 | es920_using = true; |
Sigma884 | 5:b71a554ab4c4 | 330 | es920 << HEADER_START; |
Sigma884 | 5:b71a554ab4c4 | 331 | es920 << 'R'; |
Sigma884 | 5:b71a554ab4c4 | 332 | es920.send(); |
Sigma884 | 5:b71a554ab4c4 | 333 | es920_using = false; |
Sigma884 | 2:a443df6115bc | 334 | |
Sigma884 | 5:b71a554ab4c4 | 335 | stopRec(); |
Sigma884 | 5:b71a554ab4c4 | 336 | startRecSlow(); |
Sigma884 | 5:b71a554ab4c4 | 337 | |
Sigma884 | 5:b71a554ab4c4 | 338 | controll_yn = false; |
Sigma884 | 5:b71a554ab4c4 | 339 | |
Sigma884 | 5:b71a554ab4c4 | 340 | Buzzer.fire_on(); |
Sigma884 | 5:b71a554ab4c4 | 341 | |
Sigma884 | 5:b71a554ab4c4 | 342 | do_first = true; |
Sigma884 | 2:a443df6115bc | 343 | } |
Sigma884 | 2:a443df6115bc | 344 | break; |
Sigma884 | 2:a443df6115bc | 345 | } |
Sigma884 | 2:a443df6115bc | 346 | } |
Sigma884 | 2:a443df6115bc | 347 | |
Sigma884 | 5:b71a554ab4c4 | 348 | void changePhase_RECOVERY(){ |
Sigma884 | 5:b71a554ab4c4 | 349 | CanSat_phase = CANSAT_RECOVERY; |
Sigma884 | 5:b71a554ab4c4 | 350 | do_first = false; |
Sigma884 | 5:b71a554ab4c4 | 351 | } |
Sigma884 | 5:b71a554ab4c4 | 352 | |
Sigma884 | 5:b71a554ab4c4 | 353 | void startControllAttitude(){ |
Sigma884 | 5:b71a554ab4c4 | 354 | controll_yn = true; |
Sigma884 | 5:b71a554ab4c4 | 355 | } |
Sigma884 | 5:b71a554ab4c4 | 356 | |
Sigma884 | 6:f2016544b9e4 | 357 | |
Sigma884 | 2:a443df6115bc | 358 | /******************************************************************************* |
Sigma884 | 2:a443df6115bc | 359 | 姿勢制御 |
Sigma884 | 2:a443df6115bc | 360 | *******************************************************************************/ |
Sigma884 | 2:a443df6115bc | 361 | void controllAttitude(){ |
Sigma884 | 6:f2016544b9e4 | 362 | if(controll_yn){ |
Sigma884 | 6:f2016544b9e4 | 363 | |
Sigma884 | 6:f2016544b9e4 | 364 | } |
Sigma884 | 1:6dea30c8b406 | 365 | } |
Sigma884 | 0:03be138291de | 366 | |
Sigma884 | 0:03be138291de | 367 | |
Sigma884 | 0:03be138291de | 368 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 369 | センサー読み取り |
Sigma884 | 0:03be138291de | 370 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 371 | void readSensor(){ |
Sigma884 | 1:6dea30c8b406 | 372 | time_read = time_main.read_ms(); |
Sigma884 | 1:6dea30c8b406 | 373 | time_min = time_reset * 30 + floor((double)time_read / (60 * 1000)); |
Sigma884 | 1:6dea30c8b406 | 374 | time_sec = time_read % (60 * 1000); |
Sigma884 | 1:6dea30c8b406 | 375 | time_sec = floor((double)time_sec / 1000); |
Sigma884 | 1:6dea30c8b406 | 376 | time_ms = time_read % 1000; |
Sigma884 | 1:6dea30c8b406 | 377 | if(time_read >= 30*60*1000){ |
Sigma884 | 1:6dea30c8b406 | 378 | time_main.reset(); |
Sigma884 | 1:6dea30c8b406 | 379 | time_reset ++; |
Sigma884 | 1:6dea30c8b406 | 380 | } |
Sigma884 | 1:6dea30c8b406 | 381 | |
Sigma884 | 1:6dea30c8b406 | 382 | if(CanSat_phase >= CANSAT_FLIGHT){ |
Sigma884 | 1:6dea30c8b406 | 383 | flight_time_read_old = flight_time_read; |
Sigma884 | 1:6dea30c8b406 | 384 | flight_time_read = time_flight.read_ms(); |
Sigma884 | 1:6dea30c8b406 | 385 | } |
Sigma884 | 1:6dea30c8b406 | 386 | else{ |
Sigma884 | 1:6dea30c8b406 | 387 | flight_time_read = 0; |
Sigma884 | 1:6dea30c8b406 | 388 | } |
Sigma884 | 1:6dea30c8b406 | 389 | flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000)); |
Sigma884 | 1:6dea30c8b406 | 390 | flight_time_sec = flight_time_read % (60 * 1000); |
Sigma884 | 1:6dea30c8b406 | 391 | flight_time_sec = floor((double)flight_time_sec / 1000); |
Sigma884 | 1:6dea30c8b406 | 392 | flight_time_ms = flight_time_read % 1000; |
Sigma884 | 1:6dea30c8b406 | 393 | if(time_read >= 30*60*1000){ |
Sigma884 | 1:6dea30c8b406 | 394 | time_flight.reset(); |
Sigma884 | 1:6dea30c8b406 | 395 | flight_time_reset ++; |
Sigma884 | 1:6dea30c8b406 | 396 | } |
Sigma884 | 1:6dea30c8b406 | 397 | |
Sigma884 | 1:6dea30c8b406 | 398 | ADXL375.getOutput(adxl_acc); |
Sigma884 | 1:6dea30c8b406 | 399 | |
Sigma884 | 1:6dea30c8b406 | 400 | INA226.get_Voltage_current(&ina_v, &ina_i); |
Sigma884 | 1:6dea30c8b406 | 401 | ina_v = ina_v / 1000; |
Sigma884 | 1:6dea30c8b406 | 402 | ina_i = ina_i / 1000; |
Sigma884 | 1:6dea30c8b406 | 403 | if(ina_i > 64){ |
Sigma884 | 1:6dea30c8b406 | 404 | ina_i = 0.0f; |
Sigma884 | 1:6dea30c8b406 | 405 | } |
Sigma884 | 1:6dea30c8b406 | 406 | |
Sigma884 | 1:6dea30c8b406 | 407 | BME280.getData(&temp, &pres_now, &hum); |
Sigma884 | 1:6dea30c8b406 | 408 | if(pres_now != pres){ |
Sigma884 | 1:6dea30c8b406 | 409 | pres = pres_now; |
Sigma884 | 1:6dea30c8b406 | 410 | alt = BME280.getAlt2(pres_0, temp_0); |
Sigma884 | 1:6dea30c8b406 | 411 | |
Sigma884 | 1:6dea30c8b406 | 412 | alt_buf[alt_count] = alt; |
Sigma884 | 1:6dea30c8b406 | 413 | alt_count ++; |
Sigma884 | 1:6dea30c8b406 | 414 | if(alt_count > 10){ |
Sigma884 | 1:6dea30c8b406 | 415 | alt_count = 0; |
Sigma884 | 1:6dea30c8b406 | 416 | } |
Sigma884 | 1:6dea30c8b406 | 417 | float alt_sum = 0; |
Sigma884 | 1:6dea30c8b406 | 418 | for(int i = 0; i < 10; i ++){ |
Sigma884 | 1:6dea30c8b406 | 419 | alt_sum += alt_buf[i]; |
Sigma884 | 1:6dea30c8b406 | 420 | } |
Sigma884 | 1:6dea30c8b406 | 421 | alt_ave_old = alt_ave; |
Sigma884 | 1:6dea30c8b406 | 422 | alt_ave = alt_sum / 10; |
Sigma884 | 1:6dea30c8b406 | 423 | |
Sigma884 | 1:6dea30c8b406 | 424 | if(fabs(alt_ave - alt_ave_old) > 0.01){ |
Sigma884 | 1:6dea30c8b406 | 425 | speed = (alt_ave - alt_ave_old) / (((float)flight_time_read - (float)speed_time_old) / 1000); |
Sigma884 | 1:6dea30c8b406 | 426 | } |
Sigma884 | 1:6dea30c8b406 | 427 | if(CanSat_phase <= CANSAT_WAIT){ |
Sigma884 | 1:6dea30c8b406 | 428 | speed = 0.0f; |
Sigma884 | 1:6dea30c8b406 | 429 | } |
Sigma884 | 1:6dea30c8b406 | 430 | |
Sigma884 | 1:6dea30c8b406 | 431 | speed_buf[speed_count] = speed; |
Sigma884 | 1:6dea30c8b406 | 432 | speed_count ++; |
Sigma884 | 1:6dea30c8b406 | 433 | if(speed_count > 10){ |
Sigma884 | 1:6dea30c8b406 | 434 | speed_count = 0; |
Sigma884 | 1:6dea30c8b406 | 435 | } |
Sigma884 | 1:6dea30c8b406 | 436 | float speed_sum = 0; |
Sigma884 | 1:6dea30c8b406 | 437 | for(int i = 0; i < 10; i ++){ |
Sigma884 | 1:6dea30c8b406 | 438 | speed_sum = speed_buf[i]; |
Sigma884 | 1:6dea30c8b406 | 439 | } |
Sigma884 | 1:6dea30c8b406 | 440 | speed_ave = speed_sum / 10; |
Sigma884 | 1:6dea30c8b406 | 441 | |
Sigma884 | 1:6dea30c8b406 | 442 | speed_time_old = flight_time_read; |
Sigma884 | 1:6dea30c8b406 | 443 | } |
Sigma884 | 1:6dea30c8b406 | 444 | |
Sigma884 | 1:6dea30c8b406 | 445 | if(CanSat_phase <= CANSAT_WAIT){ |
Sigma884 | 1:6dea30c8b406 | 446 | pres_0 = pres; |
Sigma884 | 1:6dea30c8b406 | 447 | temp_0 = temp; |
Sigma884 | 1:6dea30c8b406 | 448 | } |
Sigma884 | 1:6dea30c8b406 | 449 | |
Sigma884 | 1:6dea30c8b406 | 450 | BNO055.getAMG(amg); |
Sigma884 | 1:6dea30c8b406 | 451 | BNO055.getQuart(quart); |
Sigma884 | 1:6dea30c8b406 | 452 | BNO055.getEulerFromQ(euler); |
Sigma884 | 1:6dea30c8b406 | 453 | |
Sigma884 | 1:6dea30c8b406 | 454 | CCS811.getData(&CO2, &TVOCs); |
Sigma884 | 1:6dea30c8b406 | 455 | |
Sigma884 | 1:6dea30c8b406 | 456 | bottle_pres_bit = ADS1115.readADC_SingleEnded(0); |
Sigma884 | 1:6dea30c8b406 | 457 | bottle_pres_bit = (float)bottle_pres_bit * 0.125f * 3.0f / 2.0f; |
Sigma884 | 1:6dea30c8b406 | 458 | bottle_pres = (float)bottle_pres_bit/1000.0f / 4.0f - 0.25f; |
Sigma884 | 2:a443df6115bc | 459 | |
Sigma884 | 2:a443df6115bc | 460 | flight_pin = FlightPin; |
Sigma884 | 1:6dea30c8b406 | 461 | } |
Sigma884 | 0:03be138291de | 462 | |
Sigma884 | 0:03be138291de | 463 | |
Sigma884 | 0:03be138291de | 464 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 465 | GPS読み取り |
Sigma884 | 0:03be138291de | 466 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 467 | void readGPS(){ |
Sigma884 | 1:6dea30c8b406 | 468 | gps_yn = GPS.gps_readable; |
Sigma884 | 1:6dea30c8b406 | 469 | if(gps_yn){ |
Sigma884 | 1:6dea30c8b406 | 470 | gps_lat = GPS.Latitude(); |
Sigma884 | 1:6dea30c8b406 | 471 | gps_lon = GPS.Longitude(); |
Sigma884 | 1:6dea30c8b406 | 472 | gps_alt = GPS.Height(); |
Sigma884 | 1:6dea30c8b406 | 473 | GPS.getUTC(gps_utc); |
Sigma884 | 1:6dea30c8b406 | 474 | gps_utc[3] += 9; |
Sigma884 | 1:6dea30c8b406 | 475 | if(gps_utc[3] >= 24){ |
Sigma884 | 1:6dea30c8b406 | 476 | gps_utc[3] -= 24; |
Sigma884 | 1:6dea30c8b406 | 477 | gps_utc[2] += 1; |
Sigma884 | 1:6dea30c8b406 | 478 | } |
Sigma884 | 1:6dea30c8b406 | 479 | gps_knot = GPS.Knot(); |
Sigma884 | 1:6dea30c8b406 | 480 | gps_deg = GPS.Degree(); |
Sigma884 | 1:6dea30c8b406 | 481 | gps_sat = GPS.Number(); |
Sigma884 | 1:6dea30c8b406 | 482 | } |
Sigma884 | 1:6dea30c8b406 | 483 | } |
Sigma884 | 0:03be138291de | 484 | |
Sigma884 | 0:03be138291de | 485 | |
Sigma884 | 0:03be138291de | 486 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 487 | データ表示 |
Sigma884 | 0:03be138291de | 488 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 489 | void printData(){ |
Sigma884 | 2:a443df6115bc | 490 | if(ok_PC){ |
Sigma884 | 2:a443df6115bc | 491 | pc.printf("Time : %d:%02d\r\n", time_min, time_sec); |
Sigma884 | 2:a443df6115bc | 492 | pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec); |
Sigma884 | 2:a443df6115bc | 493 | |
Sigma884 | 2:a443df6115bc | 494 | pc.printf("Phase : "); |
Sigma884 | 2:a443df6115bc | 495 | switch(CanSat_phase){ |
Sigma884 | 2:a443df6115bc | 496 | case CANSAT_SETUP: |
Sigma884 | 2:a443df6115bc | 497 | pc.printf("SETUP\r\n"); |
Sigma884 | 2:a443df6115bc | 498 | break; |
Sigma884 | 2:a443df6115bc | 499 | case CANSAT_SAFETY: |
Sigma884 | 2:a443df6115bc | 500 | pc.printf("SAFETY\r\n"); |
Sigma884 | 2:a443df6115bc | 501 | break; |
Sigma884 | 2:a443df6115bc | 502 | case CANSAT_WAIT: |
Sigma884 | 2:a443df6115bc | 503 | pc.printf("WAIT\r\n"); |
Sigma884 | 2:a443df6115bc | 504 | break; |
Sigma884 | 2:a443df6115bc | 505 | case CANSAT_FLIGHT: |
Sigma884 | 2:a443df6115bc | 506 | pc.printf("FLIGHT\r\n"); |
Sigma884 | 2:a443df6115bc | 507 | break; |
Sigma884 | 2:a443df6115bc | 508 | case CANSAT_RECOVERY: |
Sigma884 | 2:a443df6115bc | 509 | pc.printf("RECOVERY\r\n"); |
Sigma884 | 2:a443df6115bc | 510 | break; |
Sigma884 | 2:a443df6115bc | 511 | } |
Sigma884 | 2:a443df6115bc | 512 | |
Sigma884 | 2:a443df6115bc | 513 | pc.printf("SAVE : "); |
Sigma884 | 2:a443df6115bc | 514 | if(save_slow){ |
Sigma884 | 2:a443df6115bc | 515 | pc.printf("SLOW\r\n"); |
Sigma884 | 2:a443df6115bc | 516 | } |
Sigma884 | 2:a443df6115bc | 517 | else if(save_fast){ |
Sigma884 | 2:a443df6115bc | 518 | pc.printf("FAST\r\n"); |
Sigma884 | 2:a443df6115bc | 519 | } |
Sigma884 | 2:a443df6115bc | 520 | else{ |
Sigma884 | 2:a443df6115bc | 521 | pc.printf("STOP\r\n"); |
Sigma884 | 2:a443df6115bc | 522 | } |
Sigma884 | 2:a443df6115bc | 523 | |
Sigma884 | 2:a443df6115bc | 524 | pc.printf("Controll : %d\r\n", controll_yn); |
Sigma884 | 2:a443df6115bc | 525 | |
Sigma884 | 2:a443df6115bc | 526 | pc.printf("Flight Pin : %d\r\n", flight_pin); |
Sigma884 | 3:4571111a5996 | 527 | pc.printf("Valve: %d, %d, %d, %d\r\n", Valve1.status, Valve2.status, Valve3.status, Valve4.status); |
Sigma884 | 3:4571111a5996 | 528 | pc.printf("Buzzer : %d\r\n", Buzzer.status); |
Sigma884 | 2:a443df6115bc | 529 | |
Sigma884 | 2:a443df6115bc | 530 | pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt); |
Sigma884 | 2:a443df6115bc | 531 | pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg); |
Sigma884 | 2:a443df6115bc | 532 | pc.printf("Sats : %d\r\n", gps_sat); |
Sigma884 | 2:a443df6115bc | 533 | |
Sigma884 | 2:a443df6115bc | 534 | pc.printf("INA226 : %.2f[V], %.2f[A]\r\n", ina_v, ina_i); |
Sigma884 | 2:a443df6115bc | 535 | pc.printf("BME280 : %.2f[hPa], %.2f[degC], %.2f[%%], %.2f[m], %.2f[m/s]\r\n", pres, temp, hum, alt_ave, speed_ave); |
Sigma884 | 2:a443df6115bc | 536 | pc.printf("BNO055 : %f, %f, %f\r\n", euler[0], euler[1], euler[2]); |
Sigma884 | 2:a443df6115bc | 537 | pc.printf("CCS811 : %d[ppm], %d[ppm]\r\n", CO2, TVOCs); |
Sigma884 | 2:a443df6115bc | 538 | pc.printf("Bottle : %f[MPa]\r\n", bottle_pres); |
Sigma884 | 2:a443df6115bc | 539 | |
Sigma884 | 2:a443df6115bc | 540 | pc.printf("\n\n\n"); |
Sigma884 | 2:a443df6115bc | 541 | } |
Sigma884 | 1:6dea30c8b406 | 542 | } |
Sigma884 | 0:03be138291de | 543 | |
Sigma884 | 0:03be138291de | 544 | |
Sigma884 | 0:03be138291de | 545 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 546 | PC読み取り |
Sigma884 | 0:03be138291de | 547 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 548 | void readPC(){ |
Sigma884 | 2:a443df6115bc | 549 | if(ok_PC){ |
Sigma884 | 2:a443df6115bc | 550 | char command = pc.getc(); |
Sigma884 | 2:a443df6115bc | 551 | pc.printf("PC Command : %c\r\n", command); |
Sigma884 | 2:a443df6115bc | 552 | |
Sigma884 | 2:a443df6115bc | 553 | switch(command){ |
Sigma884 | 2:a443df6115bc | 554 | case '?': //ヘルプ表示 |
Sigma884 | 2:a443df6115bc | 555 | printHelp(); |
Sigma884 | 2:a443df6115bc | 556 | break; |
Sigma884 | 2:a443df6115bc | 557 | |
Sigma884 | 2:a443df6115bc | 558 | case 'W': //投下待機モードへ移行 |
Sigma884 | 2:a443df6115bc | 559 | if(CanSat_phase == CANSAT_SAFETY){ |
Sigma884 | 2:a443df6115bc | 560 | CanSat_phase = CANSAT_WAIT; |
Sigma884 | 2:a443df6115bc | 561 | do_first = false; |
Sigma884 | 2:a443df6115bc | 562 | } |
Sigma884 | 2:a443df6115bc | 563 | break; |
Sigma884 | 2:a443df6115bc | 564 | |
Sigma884 | 2:a443df6115bc | 565 | case 'S': //安全モードへ移行 |
Sigma884 | 2:a443df6115bc | 566 | if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){ |
Sigma884 | 2:a443df6115bc | 567 | CanSat_phase = CANSAT_SAFETY; |
Sigma884 | 2:a443df6115bc | 568 | do_first = false; |
Sigma884 | 2:a443df6115bc | 569 | } |
Sigma884 | 2:a443df6115bc | 570 | break; |
Sigma884 | 2:a443df6115bc | 571 | |
Sigma884 | 2:a443df6115bc | 572 | case 'F': //フライトモードへ移行 |
Sigma884 | 2:a443df6115bc | 573 | if(CanSat_phase == CANSAT_WAIT){ |
Sigma884 | 2:a443df6115bc | 574 | CanSat_phase = CANSAT_FLIGHT; |
Sigma884 | 2:a443df6115bc | 575 | do_first = false; |
Sigma884 | 2:a443df6115bc | 576 | } |
Sigma884 | 2:a443df6115bc | 577 | break; |
Sigma884 | 2:a443df6115bc | 578 | |
Sigma884 | 2:a443df6115bc | 579 | case 'C': //記録停止 |
Sigma884 | 2:a443df6115bc | 580 | if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){ |
Sigma884 | 2:a443df6115bc | 581 | stopRec(); |
Sigma884 | 2:a443df6115bc | 582 | } |
Sigma884 | 2:a443df6115bc | 583 | break; |
Sigma884 | 2:a443df6115bc | 584 | |
Sigma884 | 2:a443df6115bc | 585 | case 'o': //低速記録開始 |
Sigma884 | 2:a443df6115bc | 586 | if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ |
Sigma884 | 2:a443df6115bc | 587 | startRecSlow(); |
Sigma884 | 2:a443df6115bc | 588 | } |
Sigma884 | 2:a443df6115bc | 589 | break; |
Sigma884 | 2:a443df6115bc | 590 | |
Sigma884 | 2:a443df6115bc | 591 | case 'O': //高速記録開始 |
Sigma884 | 2:a443df6115bc | 592 | if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ |
Sigma884 | 2:a443df6115bc | 593 | startRecFast(); |
Sigma884 | 2:a443df6115bc | 594 | } |
Sigma884 | 2:a443df6115bc | 595 | break; |
Sigma884 | 2:a443df6115bc | 596 | |
Sigma884 | 2:a443df6115bc | 597 | case 'B': //ブザー切り替え |
Sigma884 | 2:a443df6115bc | 598 | buzzerChange(); |
Sigma884 | 2:a443df6115bc | 599 | break; |
Sigma884 | 2:a443df6115bc | 600 | |
Sigma884 | 2:a443df6115bc | 601 | case '1': //姿勢制御再開 |
Sigma884 | 2:a443df6115bc | 602 | if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){ |
Sigma884 | 2:a443df6115bc | 603 | controll_yn = true; |
Sigma884 | 2:a443df6115bc | 604 | } |
Sigma884 | 2:a443df6115bc | 605 | break; |
Sigma884 | 2:a443df6115bc | 606 | |
Sigma884 | 2:a443df6115bc | 607 | case '0': //姿勢制御停止 |
Sigma884 | 2:a443df6115bc | 608 | if(CanSat_phase == CANSAT_FLIGHT && controll_yn){ |
Sigma884 | 2:a443df6115bc | 609 | controll_yn = false; |
Sigma884 | 2:a443df6115bc | 610 | } |
Sigma884 | 2:a443df6115bc | 611 | break; |
Sigma884 | 2:a443df6115bc | 612 | } |
Sigma884 | 2:a443df6115bc | 613 | } |
Sigma884 | 1:6dea30c8b406 | 614 | } |
Sigma884 | 0:03be138291de | 615 | |
Sigma884 | 0:03be138291de | 616 | |
Sigma884 | 0:03be138291de | 617 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 618 | ヘルプ表示 |
Sigma884 | 0:03be138291de | 619 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 620 | void printHelp(){ |
Sigma884 | 2:a443df6115bc | 621 | pc.printf("\r\n********************\r\n"); |
Sigma884 | 2:a443df6115bc | 622 | pc.printf("Commands\r\n"); |
Sigma884 | 1:6dea30c8b406 | 623 | |
Sigma884 | 2:a443df6115bc | 624 | pc.printf("W : Safety -> Wait\r\n"); |
Sigma884 | 2:a443df6115bc | 625 | pc.printf("S : Wait -> Safety\r\n"); |
Sigma884 | 2:a443df6115bc | 626 | pc.printf("F : Wait -> Flight\r\n"); |
Sigma884 | 2:a443df6115bc | 627 | pc.printf("C : Stop Recording\r\n"); |
Sigma884 | 2:a443df6115bc | 628 | pc.printf("o(small o) : Start Recording (Slow)\r\n"); |
Sigma884 | 2:a443df6115bc | 629 | pc.printf("O(large o) : Start Recording (Fast)\r\n"); |
Sigma884 | 2:a443df6115bc | 630 | pc.printf("B : Buzzer Change\r\n"); |
Sigma884 | 2:a443df6115bc | 631 | pc.printf("1(one) : Restart Controll Attitude\r\n"); |
Sigma884 | 2:a443df6115bc | 632 | pc.printf("0(zero) : Stop Controll Attitude\r\n"); |
Sigma884 | 2:a443df6115bc | 633 | |
Sigma884 | 2:a443df6115bc | 634 | pc.printf("********************\r\n\n"); |
Sigma884 | 2:a443df6115bc | 635 | wait(1.0f); |
Sigma884 | 1:6dea30c8b406 | 636 | } |
Sigma884 | 0:03be138291de | 637 | |
Sigma884 | 0:03be138291de | 638 | |
Sigma884 | 0:03be138291de | 639 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 640 | ダウンリンク送信 |
Sigma884 | 0:03be138291de | 641 | 無線あり:HEADER_DATA |
Sigma884 | 0:03be138291de | 642 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 643 | void sendDownLink(){ |
Sigma884 | 2:a443df6115bc | 644 | if(!es920_using){ |
Sigma884 | 2:a443df6115bc | 645 | es920 << HEADER_DATA; |
Sigma884 | 2:a443df6115bc | 646 | |
Sigma884 | 4:b878dce9c247 | 647 | es920 << (unsigned short)time_reset; |
Sigma884 | 4:b878dce9c247 | 648 | es920 << (unsigned short)(time_read / 1000); |
Sigma884 | 2:a443df6115bc | 649 | |
Sigma884 | 4:b878dce9c247 | 650 | es920 << (unsigned short)flight_time_reset; |
Sigma884 | 4:b878dce9c247 | 651 | es920 << (unsigned short)(flight_time_read / 1000); |
Sigma884 | 2:a443df6115bc | 652 | |
Sigma884 | 2:a443df6115bc | 653 | es920 << CanSat_phase; |
Sigma884 | 2:a443df6115bc | 654 | |
Sigma884 | 2:a443df6115bc | 655 | char status = 0x00; |
Sigma884 | 2:a443df6115bc | 656 | if(save_slow) status |= (char)0x01; |
Sigma884 | 2:a443df6115bc | 657 | if(save_fast) status |= (char)0x02; |
Sigma884 | 2:a443df6115bc | 658 | if(controll_yn) status |= (char)0x04; |
Sigma884 | 2:a443df6115bc | 659 | if(flight_pin) status |= (char)0x08; |
Sigma884 | 3:4571111a5996 | 660 | if(Buzzer.status) status |= (char)0x10; |
Sigma884 | 2:a443df6115bc | 661 | es920 << status; |
Sigma884 | 2:a443df6115bc | 662 | |
Sigma884 | 3:4571111a5996 | 663 | if(Valve1.status) status |= (char)0x01; |
Sigma884 | 3:4571111a5996 | 664 | if(Valve2.status) status |= (char)0x02; |
Sigma884 | 3:4571111a5996 | 665 | if(Valve3.status) status |= (char)0x04; |
Sigma884 | 3:4571111a5996 | 666 | if(Valve4.status) status |= (char)0x08; |
Sigma884 | 2:a443df6115bc | 667 | es920 << status; |
Sigma884 | 2:a443df6115bc | 668 | |
Sigma884 | 2:a443df6115bc | 669 | es920 << (float)gps_lat; |
Sigma884 | 2:a443df6115bc | 670 | es920 << (float)gps_lon; |
Sigma884 | 2:a443df6115bc | 671 | es920 << (unsigned short)(gps_alt / ES920_MAX_1500); |
Sigma884 | 2:a443df6115bc | 672 | es920 << (unsigned short)(gps_knot / ES920_MAX_200); |
Sigma884 | 2:a443df6115bc | 673 | es920 << (unsigned short)(gps_deg / ES920_MAX_500); |
Sigma884 | 4:b878dce9c247 | 674 | es920 << (unsigned short)gps_sat; |
Sigma884 | 2:a443df6115bc | 675 | |
Sigma884 | 2:a443df6115bc | 676 | es920 << (unsigned short)(ina_v / ES920_MAX_20); |
Sigma884 | 2:a443df6115bc | 677 | es920 << (unsigned short)(ina_i / ES920_MAX_20); |
Sigma884 | 2:a443df6115bc | 678 | |
Sigma884 | 2:a443df6115bc | 679 | es920 << (unsigned short)(pres / ES920_MAX_1500); |
Sigma884 | 2:a443df6115bc | 680 | es920 << (unsigned short)(temp / ES920_MAX_100); |
Sigma884 | 4:b878dce9c247 | 681 | es920 << (short)(alt / ES920_MAX_500); |
Sigma884 | 4:b878dce9c247 | 682 | es920 << (short)(speed / ES920_MAX_50); |
Sigma884 | 2:a443df6115bc | 683 | |
Sigma884 | 2:a443df6115bc | 684 | es920 << (short)(euler[0] / ES920_MAX_5); |
Sigma884 | 2:a443df6115bc | 685 | es920 << (short)(euler[1] / ES920_MAX_5); |
Sigma884 | 2:a443df6115bc | 686 | es920 << (short)(euler[2] / ES920_MAX_5); |
Sigma884 | 2:a443df6115bc | 687 | |
Sigma884 | 2:a443df6115bc | 688 | es920 << (unsigned short)CO2; |
Sigma884 | 2:a443df6115bc | 689 | es920 << (unsigned short)TVOCs; |
Sigma884 | 2:a443df6115bc | 690 | |
Sigma884 | 2:a443df6115bc | 691 | es920 << (unsigned short)(bottle_pres / ES920_MAX_2); |
Sigma884 | 2:a443df6115bc | 692 | |
Sigma884 | 2:a443df6115bc | 693 | es920.send(); |
Sigma884 | 2:a443df6115bc | 694 | } |
Sigma884 | 1:6dea30c8b406 | 695 | } |
Sigma884 | 0:03be138291de | 696 | |
Sigma884 | 0:03be138291de | 697 | |
Sigma884 | 0:03be138291de | 698 | /******************************************************************************* |
Sigma884 | 3:4571111a5996 | 699 | アップリンク解析 |
Sigma884 | 3:4571111a5996 | 700 | 無線あり:HEADER_COMMAND(受信) |
Sigma884 | 0:03be138291de | 701 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 702 | void readUpLink(){ |
Sigma884 | 3:4571111a5996 | 703 | es920_uplink_command = es920.data[0]; |
Sigma884 | 3:4571111a5996 | 704 | pc.printf("GND Command = %c\r\n", es920_uplink_command); |
Sigma884 | 1:6dea30c8b406 | 705 | |
Sigma884 | 3:4571111a5996 | 706 | switch(es920_uplink_command){ |
Sigma884 | 3:4571111a5996 | 707 | case 'W': //投下待機モードへ移行 |
Sigma884 | 3:4571111a5996 | 708 | if(CanSat_phase == CANSAT_SAFETY){ |
Sigma884 | 3:4571111a5996 | 709 | CanSat_phase = CANSAT_WAIT; |
Sigma884 | 3:4571111a5996 | 710 | do_first = false; |
Sigma884 | 3:4571111a5996 | 711 | } |
Sigma884 | 3:4571111a5996 | 712 | break; |
Sigma884 | 3:4571111a5996 | 713 | |
Sigma884 | 3:4571111a5996 | 714 | case 'S': //安全モードへ移行 |
Sigma884 | 3:4571111a5996 | 715 | if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){ |
Sigma884 | 3:4571111a5996 | 716 | CanSat_phase = CANSAT_SAFETY; |
Sigma884 | 3:4571111a5996 | 717 | do_first = false; |
Sigma884 | 3:4571111a5996 | 718 | } |
Sigma884 | 3:4571111a5996 | 719 | break; |
Sigma884 | 3:4571111a5996 | 720 | |
Sigma884 | 3:4571111a5996 | 721 | case 'F': //フライトモードへ移行 |
Sigma884 | 3:4571111a5996 | 722 | if(CanSat_phase == CANSAT_WAIT){ |
Sigma884 | 3:4571111a5996 | 723 | CanSat_phase = CANSAT_FLIGHT; |
Sigma884 | 3:4571111a5996 | 724 | do_first = false; |
Sigma884 | 3:4571111a5996 | 725 | } |
Sigma884 | 3:4571111a5996 | 726 | break; |
Sigma884 | 3:4571111a5996 | 727 | |
Sigma884 | 3:4571111a5996 | 728 | case 'C': //記録停止 |
Sigma884 | 3:4571111a5996 | 729 | if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){ |
Sigma884 | 3:4571111a5996 | 730 | stopRec(); |
Sigma884 | 3:4571111a5996 | 731 | } |
Sigma884 | 3:4571111a5996 | 732 | break; |
Sigma884 | 3:4571111a5996 | 733 | |
Sigma884 | 3:4571111a5996 | 734 | case 'o': //低速記録開始 |
Sigma884 | 3:4571111a5996 | 735 | if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ |
Sigma884 | 3:4571111a5996 | 736 | startRecSlow(); |
Sigma884 | 3:4571111a5996 | 737 | } |
Sigma884 | 3:4571111a5996 | 738 | break; |
Sigma884 | 3:4571111a5996 | 739 | |
Sigma884 | 3:4571111a5996 | 740 | case 'O': //高速記録開始 |
Sigma884 | 3:4571111a5996 | 741 | if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ |
Sigma884 | 3:4571111a5996 | 742 | startRecFast(); |
Sigma884 | 3:4571111a5996 | 743 | } |
Sigma884 | 3:4571111a5996 | 744 | break; |
Sigma884 | 3:4571111a5996 | 745 | |
Sigma884 | 3:4571111a5996 | 746 | case 'B': //ブザー切り替え |
Sigma884 | 3:4571111a5996 | 747 | buzzerChange(); |
Sigma884 | 3:4571111a5996 | 748 | break; |
Sigma884 | 3:4571111a5996 | 749 | |
Sigma884 | 3:4571111a5996 | 750 | case '1': //姿勢制御再開 |
Sigma884 | 3:4571111a5996 | 751 | if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){ |
Sigma884 | 3:4571111a5996 | 752 | controll_yn = true; |
Sigma884 | 3:4571111a5996 | 753 | } |
Sigma884 | 3:4571111a5996 | 754 | break; |
Sigma884 | 3:4571111a5996 | 755 | |
Sigma884 | 3:4571111a5996 | 756 | case '0': //姿勢制御停止 |
Sigma884 | 3:4571111a5996 | 757 | if(CanSat_phase == CANSAT_FLIGHT && controll_yn){ |
Sigma884 | 3:4571111a5996 | 758 | controll_yn = false; |
Sigma884 | 3:4571111a5996 | 759 | } |
Sigma884 | 3:4571111a5996 | 760 | break; |
Sigma884 | 3:4571111a5996 | 761 | } |
Sigma884 | 1:6dea30c8b406 | 762 | } |
Sigma884 | 0:03be138291de | 763 | |
Sigma884 | 0:03be138291de | 764 | |
Sigma884 | 0:03be138291de | 765 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 766 | データを1秒ごとに書き込み開始 |
Sigma884 | 0:03be138291de | 767 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 768 | void startRecSlow(){ |
Sigma884 | 3:4571111a5996 | 769 | if(!save_slow && !save_fast){ |
Sigma884 | 3:4571111a5996 | 770 | fp = fopen(file_name, "a"); |
Sigma884 | 3:4571111a5996 | 771 | fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,"); |
Sigma884 | 3:4571111a5996 | 772 | fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,"); |
Sigma884 | 3:4571111a5996 | 773 | fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,"); |
Sigma884 | 3:4571111a5996 | 774 | fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],Battery_V[V],Battery_I[A],"); |
Sigma884 | 3:4571111a5996 | 775 | fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],"); |
Sigma884 | 3:4571111a5996 | 776 | fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],"); |
Sigma884 | 3:4571111a5996 | 777 | fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],"); |
Sigma884 | 3:4571111a5996 | 778 | fprintf(fp, "CO2[ppm],TVOCs[ppm],"); |
Sigma884 | 3:4571111a5996 | 779 | fprintf(fp, "Bottle Pres[MPa]\r\n"); |
Sigma884 | 3:4571111a5996 | 780 | tick_save.detach(); |
Sigma884 | 3:4571111a5996 | 781 | tick_save.attach(&recData, 1.0f); |
Sigma884 | 3:4571111a5996 | 782 | save_slow = true; |
Sigma884 | 3:4571111a5996 | 783 | } |
Sigma884 | 1:6dea30c8b406 | 784 | } |
Sigma884 | 0:03be138291de | 785 | |
Sigma884 | 0:03be138291de | 786 | |
Sigma884 | 0:03be138291de | 787 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 788 | データを最速で書き込み開始 |
Sigma884 | 0:03be138291de | 789 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 790 | void startRecFast(){ |
Sigma884 | 3:4571111a5996 | 791 | if(!save_slow && !save_fast){ |
Sigma884 | 3:4571111a5996 | 792 | fp = fopen(file_name, "a"); |
Sigma884 | 3:4571111a5996 | 793 | fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,"); |
Sigma884 | 3:4571111a5996 | 794 | fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,"); |
Sigma884 | 3:4571111a5996 | 795 | fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,"); |
Sigma884 | 3:4571111a5996 | 796 | fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],Battery_V[V],Battery_I[A],"); |
Sigma884 | 3:4571111a5996 | 797 | fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],"); |
Sigma884 | 3:4571111a5996 | 798 | fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],"); |
Sigma884 | 3:4571111a5996 | 799 | fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],"); |
Sigma884 | 3:4571111a5996 | 800 | fprintf(fp, "CO2[ppm],TVOCs[ppm],"); |
Sigma884 | 3:4571111a5996 | 801 | fprintf(fp, "Bottle Pres[MPa]\r\n"); |
Sigma884 | 3:4571111a5996 | 802 | tick_save.detach(); |
Sigma884 | 3:4571111a5996 | 803 | save_fast = true; |
Sigma884 | 3:4571111a5996 | 804 | } |
Sigma884 | 1:6dea30c8b406 | 805 | } |
Sigma884 | 0:03be138291de | 806 | |
Sigma884 | 0:03be138291de | 807 | |
Sigma884 | 0:03be138291de | 808 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 809 | データ記録停止 |
Sigma884 | 0:03be138291de | 810 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 811 | void stopRec(){ |
Sigma884 | 3:4571111a5996 | 812 | if(save_slow){ |
Sigma884 | 3:4571111a5996 | 813 | save_slow = false; |
Sigma884 | 3:4571111a5996 | 814 | tick_save.detach(); |
Sigma884 | 3:4571111a5996 | 815 | fprintf(fp, "\r\n\n"); |
Sigma884 | 3:4571111a5996 | 816 | fclose(fp); |
Sigma884 | 3:4571111a5996 | 817 | } |
Sigma884 | 3:4571111a5996 | 818 | else if(save_fast){ |
Sigma884 | 3:4571111a5996 | 819 | save_fast = false; |
Sigma884 | 3:4571111a5996 | 820 | fprintf(fp, "\r\n\n"); |
Sigma884 | 3:4571111a5996 | 821 | fclose(fp); |
Sigma884 | 3:4571111a5996 | 822 | } |
Sigma884 | 1:6dea30c8b406 | 823 | } |
Sigma884 | 0:03be138291de | 824 | |
Sigma884 | 0:03be138291de | 825 | |
Sigma884 | 0:03be138291de | 826 | /******************************************************************************* |
Sigma884 | 3:4571111a5996 | 827 | データの書き込み |
Sigma884 | 0:03be138291de | 828 | *******************************************************************************/ |
Sigma884 | 1:6dea30c8b406 | 829 | void recData(){ |
Sigma884 | 3:4571111a5996 | 830 | fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms); |
Sigma884 | 3:4571111a5996 | 831 | fprintf(fp, "%d:%02d.%03d,", flight_time_min, flight_time_sec, flight_time_ms); |
Sigma884 | 1:6dea30c8b406 | 832 | |
Sigma884 | 3:4571111a5996 | 833 | switch(CanSat_phase){ |
Sigma884 | 3:4571111a5996 | 834 | case CANSAT_SETUP: |
Sigma884 | 3:4571111a5996 | 835 | fprintf(fp, "SETUP,"); |
Sigma884 | 3:4571111a5996 | 836 | break; |
Sigma884 | 3:4571111a5996 | 837 | |
Sigma884 | 3:4571111a5996 | 838 | case CANSAT_SAFETY: |
Sigma884 | 3:4571111a5996 | 839 | fprintf(fp, "SAFETY,"); |
Sigma884 | 3:4571111a5996 | 840 | break; |
Sigma884 | 3:4571111a5996 | 841 | |
Sigma884 | 3:4571111a5996 | 842 | case CANSAT_WAIT: |
Sigma884 | 3:4571111a5996 | 843 | fprintf(fp, "WAIT,"); |
Sigma884 | 3:4571111a5996 | 844 | break; |
Sigma884 | 3:4571111a5996 | 845 | |
Sigma884 | 3:4571111a5996 | 846 | case CANSAT_FLIGHT: |
Sigma884 | 3:4571111a5996 | 847 | fprintf(fp, "FLIGHT,"); |
Sigma884 | 3:4571111a5996 | 848 | break; |
Sigma884 | 3:4571111a5996 | 849 | |
Sigma884 | 3:4571111a5996 | 850 | case CANSAT_RECOVERY: |
Sigma884 | 3:4571111a5996 | 851 | fprintf(fp, "RECOVERY,"); |
Sigma884 | 3:4571111a5996 | 852 | break; |
Sigma884 | 3:4571111a5996 | 853 | } |
Sigma884 | 3:4571111a5996 | 854 | |
Sigma884 | 3:4571111a5996 | 855 | fprintf(fp, "%d,", controll_yn); |
Sigma884 | 3:4571111a5996 | 856 | fprintf(fp, "%d,", flight_pin); |
Sigma884 | 3:4571111a5996 | 857 | fprintf(fp, "%d,%d,%d,%d,%d,", Valve1.status, Valve2.status, Valve3.status, Valve4.status, Buzzer.status); |
Sigma884 | 3:4571111a5996 | 858 | fprintf(fp, "%c,", es920_uplink_command); |
Sigma884 | 3:4571111a5996 | 859 | es920_uplink_command = '-'; |
Sigma884 | 3:4571111a5996 | 860 | |
Sigma884 | 3:4571111a5996 | 861 | fprintf(fp, "%d,", gps_yn); |
Sigma884 | 3:4571111a5996 | 862 | fprintf(fp, "%d/%d/%d ", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付 |
Sigma884 | 3:4571111a5996 | 863 | fprintf(fp, "%d:%02d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間 |
Sigma884 | 3:4571111a5996 | 864 | fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標 |
Sigma884 | 3:4571111a5996 | 865 | fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度 |
Sigma884 | 3:4571111a5996 | 866 | fprintf(fp, "%d,", gps_sat); //GPS衛星数 |
Sigma884 | 3:4571111a5996 | 867 | |
Sigma884 | 3:4571111a5996 | 868 | fprintf(fp, "%.2f,%.2f,%.2f,%f,%f,", adxl_acc[0], adxl_acc[1], adxl_acc[2], ina_v, ina_i); |
Sigma884 | 3:4571111a5996 | 869 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,", pres, temp, hum, alt, alt_ave, speed, speed_ave); |
Sigma884 | 3:4571111a5996 | 870 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,", amg[0], amg[1], amg[2], amg[3], amg[4], amg[5], amg[6], amg[7], amg[8]); |
Sigma884 | 3:4571111a5996 | 871 | fprintf(fp, "%f,%f,%f,%f,", quart[0], quart[1], quart[2], quart[3]); |
Sigma884 | 3:4571111a5996 | 872 | fprintf(fp, "%.2f,%.2f,%.2f,", euler[0], euler[1], euler[2]); |
Sigma884 | 3:4571111a5996 | 873 | fprintf(fp, "%d,%d,", CO2, TVOCs); |
Sigma884 | 3:4571111a5996 | 874 | fprintf(fp, "%f\r\n", bottle_pres); |
Sigma884 | 3:4571111a5996 | 875 | |
Sigma884 | 4:b878dce9c247 | 876 | /////////////////////////////////////////////////////////////////////EEPROM |
Sigma884 | 4:b878dce9c247 | 877 | EEPROM.chargeBuff((unsigned short)time_reset); |
Sigma884 | 4:b878dce9c247 | 878 | EEPROM.chargeBuff((int)time_read); |
Sigma884 | 4:b878dce9c247 | 879 | EEPROM.chargeBuff((unsigned short)flight_time_reset); |
Sigma884 | 4:b878dce9c247 | 880 | EEPROM.chargeBuff((int)flight_time_read); |
Sigma884 | 4:b878dce9c247 | 881 | EEPROM.chargeBuff((char)CanSat_phase); |
Sigma884 | 4:b878dce9c247 | 882 | char status = 0x00; |
Sigma884 | 4:b878dce9c247 | 883 | if(controll_yn) status |= 0x01; |
Sigma884 | 4:b878dce9c247 | 884 | if(flight_pin) status |= 0x02; |
Sigma884 | 4:b878dce9c247 | 885 | if(Valve1.status) status |= 0x04; |
Sigma884 | 4:b878dce9c247 | 886 | if(Valve2.status) status |= 0x08; |
Sigma884 | 4:b878dce9c247 | 887 | if(Valve3.status) status |= 0x10; |
Sigma884 | 4:b878dce9c247 | 888 | if(Valve4.status) status |= 0x20; |
Sigma884 | 4:b878dce9c247 | 889 | if(Buzzer.status) status |= 0x40; |
Sigma884 | 4:b878dce9c247 | 890 | EEPROM.chargeBuff((char)status); |
Sigma884 | 4:b878dce9c247 | 891 | EEPROM.chargeBuff((char)es920_uplink_command); |
Sigma884 | 4:b878dce9c247 | 892 | EEPROM.chargeBuff((char)gps_yn); |
Sigma884 | 4:b878dce9c247 | 893 | EEPROM.chargeBuff((float)gps_lat); |
Sigma884 | 4:b878dce9c247 | 894 | EEPROM.chargeBuff((float)gps_lon); |
Sigma884 | 4:b878dce9c247 | 895 | EEPROM.chargeBuff((float)gps_alt); |
Sigma884 | 4:b878dce9c247 | 896 | EEPROM.chargeBuff((float)gps_knot); |
Sigma884 | 4:b878dce9c247 | 897 | EEPROM.chargeBuff((float)gps_deg); |
Sigma884 | 4:b878dce9c247 | 898 | EEPROM.chargeBuff((unsigned short)gps_sat); |
Sigma884 | 4:b878dce9c247 | 899 | EEPROM.chargeBuff((float)adxl_acc[0]); |
Sigma884 | 4:b878dce9c247 | 900 | EEPROM.chargeBuff((float)adxl_acc[1]); |
Sigma884 | 4:b878dce9c247 | 901 | EEPROM.chargeBuff((float)adxl_acc[2]); |
Sigma884 | 4:b878dce9c247 | 902 | EEPROM.chargeBuff((float)ina_v); |
Sigma884 | 4:b878dce9c247 | 903 | EEPROM.chargeBuff((float)ina_i); |
Sigma884 | 4:b878dce9c247 | 904 | EEPROM.chargeBuff((float)pres); |
Sigma884 | 4:b878dce9c247 | 905 | EEPROM.chargeBuff((float)temp); |
Sigma884 | 4:b878dce9c247 | 906 | EEPROM.chargeBuff((float)hum); |
Sigma884 | 4:b878dce9c247 | 907 | EEPROM.chargeBuff((float)alt); |
Sigma884 | 4:b878dce9c247 | 908 | EEPROM.chargeBuff((float)speed); |
Sigma884 | 4:b878dce9c247 | 909 | EEPROM.chargeBuff((short)(amg[0] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 910 | EEPROM.chargeBuff((short)(amg[1] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 911 | EEPROM.chargeBuff((short)(amg[2] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 912 | EEPROM.chargeBuff((short)(amg[3] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 913 | EEPROM.chargeBuff((short)(amg[4] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 914 | EEPROM.chargeBuff((short)(amg[5] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 915 | EEPROM.chargeBuff((short)(amg[6] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 916 | EEPROM.chargeBuff((short)(amg[7] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 917 | EEPROM.chargeBuff((short)(amg[8] / ES920_MAX_50)); |
Sigma884 | 4:b878dce9c247 | 918 | EEPROM.chargeBuff((float)quart[0]); |
Sigma884 | 4:b878dce9c247 | 919 | EEPROM.chargeBuff((float)quart[1]); |
Sigma884 | 4:b878dce9c247 | 920 | EEPROM.chargeBuff((float)quart[2]); |
Sigma884 | 4:b878dce9c247 | 921 | EEPROM.chargeBuff((float)quart[3]); |
Sigma884 | 4:b878dce9c247 | 922 | EEPROM.chargeBuff((short)(euler[0] / ES920_MAX_500)); |
Sigma884 | 4:b878dce9c247 | 923 | EEPROM.chargeBuff((short)(euler[1] / ES920_MAX_500)); |
Sigma884 | 4:b878dce9c247 | 924 | EEPROM.chargeBuff((short)(euler[2] / ES920_MAX_500)); |
Sigma884 | 4:b878dce9c247 | 925 | EEPROM.chargeBuff((unsigned short)CO2); |
Sigma884 | 4:b878dce9c247 | 926 | EEPROM.chargeBuff((unsigned short)TVOCs); |
Sigma884 | 4:b878dce9c247 | 927 | EEPROM.chargeBuff((float)bottle_pres); |
Sigma884 | 4:b878dce9c247 | 928 | EEPROM.writeBuff(); |
Sigma884 | 4:b878dce9c247 | 929 | eeprom_ptr = EEPROM.setNextPage(); |
Sigma884 | 1:6dea30c8b406 | 930 | } |
Sigma884 | 0:03be138291de | 931 | |
Sigma884 | 0:03be138291de | 932 | |
Sigma884 | 0:03be138291de | 933 | /******************************************************************************* |
Sigma884 | 2:a443df6115bc | 934 | ブザーのオンオフ切り替え |
Sigma884 | 2:a443df6115bc | 935 | *******************************************************************************/ |
Sigma884 | 2:a443df6115bc | 936 | void buzzerChange(){ |
Sigma884 | 3:4571111a5996 | 937 | if(Buzzer.status){ |
Sigma884 | 3:4571111a5996 | 938 | Buzzer.fire_off(); |
Sigma884 | 3:4571111a5996 | 939 | } |
Sigma884 | 3:4571111a5996 | 940 | else{ |
Sigma884 | 3:4571111a5996 | 941 | Buzzer.fire_off(); |
Sigma884 | 3:4571111a5996 | 942 | } |
Sigma884 | 2:a443df6115bc | 943 | } |
Sigma884 | 2:a443df6115bc | 944 | |
Sigma884 | 2:a443df6115bc | 945 | |
Sigma884 | 2:a443df6115bc | 946 | /******************************************************************************* |
Sigma884 | 0:03be138291de | 947 | セットアップ(最初に1回実行) |
Sigma884 | 0:03be138291de | 948 | 無線あり:HEADER_SETUP_SENSOR |
Sigma884 | 0:03be138291de | 949 | 無線あり:HEADER_GPS_SETUP |
Sigma884 | 0:03be138291de | 950 | *******************************************************************************/ |
Sigma884 | 0:03be138291de | 951 | void setup(){ |
Sigma884 | 0:03be138291de | 952 | wait(1.0f); |
Sigma884 | 0:03be138291de | 953 | char setup_string[1024]; |
Sigma884 | 0:03be138291de | 954 | |
Sigma884 | 0:03be138291de | 955 | pc.printf("\r\n******************************\r\n"); |
Sigma884 | 0:03be138291de | 956 | pc.printf("Sensor Setting Start!!\r\n"); |
Sigma884 | 0:03be138291de | 957 | strcat(setup_string, "Sensor Setting Start!!\r\n"); |
Sigma884 | 0:03be138291de | 958 | es920 << HEADER_SENSOR_SETUP; |
Sigma884 | 0:03be138291de | 959 | |
Sigma884 | 0:03be138291de | 960 | //////////////////////////////////////////////////ADXL375 |
Sigma884 | 0:03be138291de | 961 | ADXL375.setDataRate(ADXL375_100HZ); |
Sigma884 | 0:03be138291de | 962 | if(ADXL375.whoAmI() == 1){ |
Sigma884 | 0:03be138291de | 963 | pc.printf("ADXL375 : OK\r\n"); |
Sigma884 | 0:03be138291de | 964 | strcat(setup_string, "ADXL375 : OK\r\n"); |
Sigma884 | 0:03be138291de | 965 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 966 | } |
Sigma884 | 0:03be138291de | 967 | else{ |
Sigma884 | 0:03be138291de | 968 | pc.printf("ADXL375 : NG\r\n"); |
Sigma884 | 0:03be138291de | 969 | strcat(setup_string, "ADXL375 : NG\r\n"); |
Sigma884 | 0:03be138291de | 970 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 971 | } |
Sigma884 | 0:03be138291de | 972 | ADXL375.offset(0.0f, 0.0f, 0.0f); |
Sigma884 | 0:03be138291de | 973 | |
Sigma884 | 0:03be138291de | 974 | //////////////////////////////////////////////////INA226 |
Sigma884 | 0:03be138291de | 975 | INA226.set_callibretion(); |
Sigma884 | 0:03be138291de | 976 | if(INA226.Connection_check() == 0){ |
Sigma884 | 0:03be138291de | 977 | pc.printf("INA226 : OK\r\n"); |
Sigma884 | 0:03be138291de | 978 | strcat(setup_string, "INA226 : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 979 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 980 | } |
Sigma884 | 0:03be138291de | 981 | else{ |
Sigma884 | 0:03be138291de | 982 | pc.printf("INA226 : NG\r\n"); |
Sigma884 | 0:03be138291de | 983 | strcat(setup_string, "INA226 : NG\r\n"); |
Sigma884 | 0:03be138291de | 984 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 985 | } |
Sigma884 | 0:03be138291de | 986 | |
Sigma884 | 0:03be138291de | 987 | //////////////////////////////////////////////////BME280 |
Sigma884 | 0:03be138291de | 988 | BME280.configMeasure(BME280_lib::NORMAL, 1, 2, 1); |
Sigma884 | 0:03be138291de | 989 | BME280.configFilter(4); |
Sigma884 | 0:03be138291de | 990 | if(BME280.connectCheck() == 1){ |
Sigma884 | 0:03be138291de | 991 | pc.printf("BME280 OK!!\r\n"); |
Sigma884 | 0:03be138291de | 992 | strcat(setup_string, "BME280 : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 993 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 994 | } |
Sigma884 | 0:03be138291de | 995 | else{ |
Sigma884 | 0:03be138291de | 996 | pc.printf("BME280 NG\r\n"); |
Sigma884 | 0:03be138291de | 997 | strcat(setup_string, "BME280 : NG\r\n"); |
Sigma884 | 0:03be138291de | 998 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 999 | } |
Sigma884 | 0:03be138291de | 1000 | |
Sigma884 | 0:03be138291de | 1001 | //////////////////////////////////////////////////BNO055 |
Sigma884 | 0:03be138291de | 1002 | BNO055.setOperationMode(BNO055_lib::CONFIG); |
Sigma884 | 0:03be138291de | 1003 | BNO055.setAccRange(BNO055_lib::_16G); |
Sigma884 | 0:03be138291de | 1004 | BNO055.setAxis(BNO055_lib::Z, BNO055_lib::Y, BNO055_lib::X); |
Sigma884 | 0:03be138291de | 1005 | BNO055.setAxisPM(1, 1, -1); |
Sigma884 | 0:03be138291de | 1006 | if(BNO055.connectCheck() == 1){ |
Sigma884 | 0:03be138291de | 1007 | pc.printf("BNO055 OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1008 | strcat(setup_string, "BNO055 : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1009 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 1010 | } |
Sigma884 | 0:03be138291de | 1011 | else{ |
Sigma884 | 0:03be138291de | 1012 | pc.printf("BNO055 NG\r\n"); |
Sigma884 | 0:03be138291de | 1013 | strcat(setup_string, "BNO055 : NG\r\n"); |
Sigma884 | 0:03be138291de | 1014 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 1015 | } |
Sigma884 | 1:6dea30c8b406 | 1016 | BNO055.setOperationMode(BNO055_lib::NDOF); |
Sigma884 | 0:03be138291de | 1017 | |
Sigma884 | 0:03be138291de | 1018 | //////////////////////////////////////////////////CCS811 |
Sigma884 | 0:03be138291de | 1019 | if(CCS811.connectCheck() == 1){ |
Sigma884 | 0:03be138291de | 1020 | pc.printf("CCS811 OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1021 | strcat(setup_string, "CCS811 : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1022 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 1023 | } |
Sigma884 | 0:03be138291de | 1024 | else{ |
Sigma884 | 0:03be138291de | 1025 | pc.printf("CCS811 NG\r\n"); |
Sigma884 | 0:03be138291de | 1026 | strcat(setup_string, "CCS811 : NG\r\n"); |
Sigma884 | 0:03be138291de | 1027 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 1028 | } |
Sigma884 | 0:03be138291de | 1029 | |
Sigma884 | 0:03be138291de | 1030 | //////////////////////////////////////////////////圧力センサ |
Sigma884 | 0:03be138291de | 1031 | pc.printf("PRES : "); |
Sigma884 | 0:03be138291de | 1032 | ADS1115.setGain(GAIN_TWOTHIRDS); |
Sigma884 | 0:03be138291de | 1033 | pc.printf("OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1034 | strcat(setup_string, "PRES : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1035 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 1036 | |
Sigma884 | 0:03be138291de | 1037 | //////////////////////////////////////////////////SD |
Sigma884 | 0:03be138291de | 1038 | fp = fopen("/sd/Space_SWAN_setup.txt", "r"); |
Sigma884 | 0:03be138291de | 1039 | if(fp != NULL){ |
Sigma884 | 0:03be138291de | 1040 | pc.printf("SD : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1041 | strcat(setup_string, "SD : OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1042 | es920 << (char)0x01; |
Sigma884 | 0:03be138291de | 1043 | fclose(fp); |
Sigma884 | 0:03be138291de | 1044 | } |
Sigma884 | 0:03be138291de | 1045 | else{ |
Sigma884 | 0:03be138291de | 1046 | pc.printf("SD : NG...\r\n"); |
Sigma884 | 0:03be138291de | 1047 | strcat(setup_string, "SD : NG...\r\n"); |
Sigma884 | 0:03be138291de | 1048 | es920 << (char)0x00; |
Sigma884 | 0:03be138291de | 1049 | } |
Sigma884 | 0:03be138291de | 1050 | |
Sigma884 | 0:03be138291de | 1051 | pc.printf("Sensor Setting Finished!!\r\n"); |
Sigma884 | 0:03be138291de | 1052 | pc.printf("******************************\r\n"); |
Sigma884 | 0:03be138291de | 1053 | strcat(setup_string, "Sensor Setting Finished!!\r\n"); |
Sigma884 | 0:03be138291de | 1054 | es920.send(); |
Sigma884 | 0:03be138291de | 1055 | wait(1.0f); |
Sigma884 | 0:03be138291de | 1056 | |
Sigma884 | 0:03be138291de | 1057 | pc.printf("\r\n******************************\r\n"); |
Sigma884 | 0:03be138291de | 1058 | if(wait_GPS){ |
Sigma884 | 0:03be138291de | 1059 | pc.printf("GPS Setting Start!!\r\n"); |
Sigma884 | 0:03be138291de | 1060 | strcat(setup_string, "GPS Setting Start!!\r\n"); |
Sigma884 | 0:03be138291de | 1061 | |
Sigma884 | 0:03be138291de | 1062 | pc.printf("Waiting : 0"); |
Sigma884 | 0:03be138291de | 1063 | strcat(setup_string, "Wait : "); |
Sigma884 | 0:03be138291de | 1064 | int gps_wait_count = 0; |
Sigma884 | 0:03be138291de | 1065 | while(!GPS.gps_readable){ |
Sigma884 | 0:03be138291de | 1066 | pc.printf("\rWaiting : %d", gps_wait_count);// |
Sigma884 | 0:03be138291de | 1067 | es920 << HEADER_GPS_SETUP; |
Sigma884 | 0:03be138291de | 1068 | es920 << (char)GPS.gps_readable; |
Sigma884 | 0:03be138291de | 1069 | es920 << (int)gps_wait_count; |
Sigma884 | 0:03be138291de | 1070 | es920.send(); |
Sigma884 | 0:03be138291de | 1071 | wait(1.0f); |
Sigma884 | 0:03be138291de | 1072 | gps_wait_count ++; |
Sigma884 | 0:03be138291de | 1073 | } |
Sigma884 | 0:03be138291de | 1074 | char ss[8]; |
Sigma884 | 0:03be138291de | 1075 | sprintf(ss, "%d", gps_wait_count); |
Sigma884 | 0:03be138291de | 1076 | strcat(setup_string, ss); |
Sigma884 | 0:03be138291de | 1077 | pc.printf(" OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1078 | strcat(setup_string, " OK!!\r\n"); |
Sigma884 | 0:03be138291de | 1079 | pc.printf("GPS Setting Finished!!\r\n"); |
Sigma884 | 0:03be138291de | 1080 | strcat(setup_string, "GPS Setting Finished!!\r\n"); |
Sigma884 | 0:03be138291de | 1081 | es920 << HEADER_GPS_SETUP; |
Sigma884 | 0:03be138291de | 1082 | es920 << (char)0xAA; |
Sigma884 | 0:03be138291de | 1083 | es920 << (int)0; |
Sigma884 | 0:03be138291de | 1084 | es920.send(); |
Sigma884 | 0:03be138291de | 1085 | } |
Sigma884 | 0:03be138291de | 1086 | else{ |
Sigma884 | 0:03be138291de | 1087 | pc.printf("GPS Setting Ignore...\r\n"); |
Sigma884 | 0:03be138291de | 1088 | strcat(setup_string, "GPS Setting Ignore...\r\n"); |
Sigma884 | 0:03be138291de | 1089 | es920 << HEADER_GPS_SETUP; |
Sigma884 | 0:03be138291de | 1090 | es920 << (char)0xFF; |
Sigma884 | 0:03be138291de | 1091 | es920 << (int)0; |
Sigma884 | 0:03be138291de | 1092 | es920.send(); |
Sigma884 | 0:03be138291de | 1093 | } |
Sigma884 | 0:03be138291de | 1094 | pc.printf("******************************\r\n"); |
Sigma884 | 0:03be138291de | 1095 | fp = fopen(file_name, "a"); |
Sigma884 | 0:03be138291de | 1096 | fprintf(fp, setup_string); |
Sigma884 | 0:03be138291de | 1097 | fclose(fp); |
Sigma884 | 0:03be138291de | 1098 | setup_string[0] = NULL; |
Sigma884 | 0:03be138291de | 1099 | |
Sigma884 | 3:4571111a5996 | 1100 | EEPROM.setWriteAddr(1, 0, 0x00, 0x00); |
Sigma884 | 3:4571111a5996 | 1101 | |
Sigma884 | 0:03be138291de | 1102 | printHelp(); |
Sigma884 | 0:03be138291de | 1103 | wait(1.0f); |
Sigma884 | 0:03be138291de | 1104 | } |