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