PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated SWAN_IZU2019_v1

高高度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@8:2ce2c7eebb92, 2019-04-21 (annotated)

Committer:
Sigma884
Date:
Sun Apr 21 10:37:15 2019 +0000
Revision:
8:2ce2c7eebb92
Parent:
7:3a2d0474f1ca 
Space SWAN (CanSat) Main Program Ver.1

Who changed what in which revision?

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