PQ_Hybrid_Electrical_Equipment_Team
2021年3月伊豆大島共同打上実験の電装メインプログラム
Dependencies: PQ_ES920 mbed PQ_LPS22HB SDFileSystem PQ_EEPROM PQ_ADXL375 PQ_MPU9250 PQ_INA226 PQ_GPS
Revision 6:d951f75ce5e8, committed 2021-03-13
- Comitter:
- tanahashi
- Date:
- Sat Mar 13 12:24:45 2021 +0000
- Parent:
- 5:dc80ccf0904e
- Commit message:
- version 1.3
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r dc80ccf0904e -r d951f75ce5e8 main.cpp
--- a/main.cpp Thu Mar 11 03:45:08 2021 +0000
+++ b/main.cpp Sat Mar 13 12:24:45 2021 +0000
@@ -1,4 +1,4 @@
-/*
+ /*
* ハイブリッドロケット（2021年3月伊豆大島共同打上実験）アビオニクスメインモジュール
*/
@@ -14,7 +14,7 @@
#include "PQ_MPU9250.h"
const float BURN_TIME = 3.27f; // エンジン燃焼時間[s]
-const float T_APOGEE = 900.44f; // 離床検知から弾道頂点までの時間[s]
+const float T_APOGEE = 13.44f; // 離床検知から弾道頂点までの時間[s]
const float T_SEP = 12.0f; // 分離にかける時間[s]
const float SAMPLING_FREQUENCY = 10.0f; // データのサンプリングレート[Hz]
const float DOWNLINK_RATE = 2.0f; // ダウンリンクのレート[Hz]
@@ -31,7 +31,9 @@
ADXL375 adxl(i2c, ADXL375::ALT_ADDRESS_HIGH); // 3軸高加速度
INA226 ina_in(i2c, INA226::A0_VS, INA226::A1_VS); // 電圧，電流（バッテリー）
INA226 ina_ex(i2c, INA226::A0_GND, INA226::A1_GND); // 電圧，電流（外部電源）
-LPS22HB lps(i2c, LPS22HB::SA0_HIGH); // 気圧，温度
+//消す！！
+LPS22HB lps(i2c, LPS22HB::SA0_LOW); // 気圧，温度
+//LPS22HB lps(i2c, LPS22HB::SA0_HIGH); // 気圧，温度
MPU9250 mpu(i2c, MPU9250::AD0_HIGH); // 3軸加速度，3軸角速度，3軸地磁気
SDFileSystem sd(p5, p6, p7, p8, "sd");
@@ -46,6 +48,7 @@
Ticker record_ticker; // データ保存
DigitalIn flight(p20); // 離床検知用フライトピン（外部プルアップ）
+DigitalIn signal(p24); // ミッション基板の生存信号
DigitalOut mission(p18); // ミッション基板の動作制御
DigitalOut relay(p19); // ミッション基板のリレースイッチング
@@ -63,6 +66,7 @@
bool launched = false; // 打ちあがっているか：true=離床済み
bool burning = false; // エンジンが燃焼中か：true=燃焼中
+bool use_apogee = true; // 頂点検知を行うか：true=行う
bool apogee = false; // 頂点検知したか：true=頂点検知済み
bool separated = false; // 分離したか：true=分離済み
bool landed = false; // 着水したか：true=着水済み
@@ -159,14 +163,14 @@
burning = false;
press_LPF_prev = press_LPF;
}
- if(flight_timer.read() - t > 0.5) {
- press_LPF_diff = press_LPF - press_LPF_prev;
+ if(flight_timer.read() - t > 0.25) {
+ if(press_LPF - press_LPF_prev > 0.0f) {
+ if(use_apogee){
+ apogee = true;
+ }
+ }
press_LPF_prev = press_LPF;
- if(press_LPF_diff > 0.0f) {
- apogee = true;
- } else {
- t = flight_timer.read();
- }
+ t = flight_timer.read();
}
if(!burning && (apogee || flight_timer.read() > T_APOGEE)) {
phase = SEP;
@@ -194,12 +198,9 @@
break;
case RECOVERY:
mission = 0;
- if(!landed) {
- if(press_LPF < press_LPF_ground) {
+ if(!landed && press_LPF > press_LPF_ground) {
relay = 0;
- } else {
- relay = 1;
- }
+ landed = true;
}
sep = 0;
buzzer = sound;
@@ -230,7 +231,7 @@
if(fp) {
fprintf(fp, "mission_time,flight_time,phase,");
- fprintf(fp, "launched,apogee,separated,landed,mission,relay,sep,buzzer,");
+ fprintf(fp, "use_apogee,apogee,landed,signal,mission,relay,sep,buzzer,");
fprintf(fp, "flight,f_sd,f_gps,f_adxl,f_ina_in,f_ina_ex,f_lps,f_mpu,");
fprintf(fp, "lat,lon,sat,fix,hdop,alt,geoid,");
fprintf(fp, "high_accel_x,high_accel_y,high_accel_z,");
@@ -367,8 +368,11 @@
case 0xB5: // '5'
if(phase == SEP) phase = RECOVERY;
break;
- case 0xE2: // 'B'
- sound = 0;
+ case 0xE1: // 'a'
+ use_apogee = !use_apogee;
+ break;
+ case 0xE2: // 'b'
+ sound = !sound;
break;
case 0xFF: // 'DEL'
NVIC_SystemReset();
@@ -381,14 +385,14 @@
short mission_time_bits = (short)mission_time;;
short flight_time_bits = (short)(flight_time * 10);
char flags1 = 0;
- flags1 |= (char)launched << 7;
- flags1 |= (char)apogee << 6;
- flags1 |= (char)separated << 5;
- flags1 |= (char)landed << 4;
- flags1 |= mission.read() << 3;
- flags1 |= relay.read() << 2;
- flags1 |= sep.read() << 1;
- flags1 |= buzzer.read() << 0;
+ flags1 |= (char)use_apogee << 7;
+ flags1 |= (char)apogee << 6;
+ flags1 |= (char)landed << 5;
+ flags1 |= signal.read() << 4;
+ flags1 |= mission.read() << 3;
+ flags1 |= relay.read() << 2;
+ flags1 |= sep.read() << 1;
+ flags1 |= buzzer.read() << 0;
char flags2 = 0;
flags2 |= flight.read() << 7;
flags2 |= f_sd << 6;
@@ -488,10 +492,10 @@
data[6] = ((char*)&flight_time)[2];
data[7] = ((char*)&flight_time)[3];
data[8] = phase;
- data[9] = launched;
+ data[9] = use_apogee;
data[10] = apogee;
- data[11] = separated;
- data[12] = landed;
+ data[11] = landed;
+ data[12] = signal.read();
data[13] = mission.read();
data[14] = relay.read();
data[15] = sep.read();
@@ -615,7 +619,7 @@
if(fp) {
char *phase_names[] = {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
fprintf(fp, "%.3f,%.3f,%s,", mission_time, flight_time, phase_names[phase]);
- fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", launched, apogee, separated, landed, mission.read(), relay.read(), sep.read(), buzzer.read());
+ fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", use_apogee, apogee, landed, signal.read(), mission.read(), relay.read(), sep.read(), buzzer.read());
fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", flight.read(), f_sd, f_gps, f_adxl, f_ina_in, f_ina_ex, f_lps, f_mpu);
fprintf(fp, "%.6f,%.6f,%d,%d,%f,%f,%f,", lat, lon, sat, fix, hdop, alt, geoid);
fprintf(fp, "%f,%f,%f,", high_accel[0], high_accel[1], high_accel[2]);