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