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@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?

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 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 }