From c5dfac837cc2550ae403aeced8162b644fa305e1 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sun, 25 Sep 2022 17:22:13 +0200
Subject: [PATCH] [FlightStatsRecorder] fixed main implementation
[FlightModeManager] Implemented mission_ended state
---
.../Main/Configs/FlightModeManagerConfig.h | 3 +-
.../FlightStatsRecorder.cpp | 4 +-
.../ADAController/ADAController.cpp | 2 +
.../FlightModeManager/FlightModeManager.cpp | 58 +++++++++++++++++-
.../FlightModeManager/FlightModeManager.h | 3 +
.../FlightModeManager/FlightModeManagerData.h | 3 +-
.../NASController/NASController.cpp | 2 +
.../Payload/Configs/FlightModeManagerConfig.h | 3 +-
.../FlightModeManager/FlightModeManager.cpp | 59 ++++++++++++++++---
.../FlightModeManager/FlightModeManager.h | 3 +
.../FlightModeManager/FlightModeManagerData.h | 3 +-
src/boards/common/events/Events.h | 1 +
12 files changed, 128 insertions(+), 16 deletions(-)
diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main/Configs/FlightModeManagerConfig.h
index fbe7a40ba..857df4abf 100644
--- a/src/boards/Main/Configs/FlightModeManagerConfig.h
+++ b/src/boards/Main/Configs/FlightModeManagerConfig.h
@@ -26,5 +26,6 @@ namespace Main
{
constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000;
+constexpr unsigned int LANDING_TIMEOUT = 2 * 60 * 1000;
-}
+} // namespace Main
diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
index e9d6d70e8..a0fe06d32 100644
--- a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
+++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -81,6 +81,7 @@ void FlightStatsRecorder::update(Boardcore::ADAState state)
(state.x0 < stats.ada_min_pressure || stats.ada_min_pressure == -1))
{
stats.ada_min_pressure = state.x0;
+ stats.apogee_alt = state.aglAltitude;
}
}
@@ -112,7 +113,9 @@ void FlightStatsRecorder::update(PressureData data)
if ((fmmState.state == FlightModeManagerState::ASCENDING ||
fmmState.state == FlightModeManagerState::DROGUE_DESCENT) &&
(data.pressure < stats.min_pressure || stats.min_pressure == -1))
+ {
stats.min_pressure = data.pressure;
+ }
}
void FlightStatsRecorder::updateDplVane(PressureData data)
@@ -128,7 +131,6 @@ void FlightStatsRecorder::updateDplVane(PressureData data)
void FlightStatsRecorder::setApogee(GPSData data)
{
- stats.apogee_alt = data.height;
stats.apogee_lat = data.latitude;
stats.apogee_lon = data.longitude;
}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index c7a7418f5..039f38910 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -25,6 +25,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Configs/AirBrakesControllerConfig.h>
#include <Main/Configs/NASConfig.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/AirBrakesController/AirBrakesController.h>
#include <common/ReferenceConfig.h>
@@ -209,6 +210,7 @@ void ADAController::update()
}
Logger::getInstance().log(ada.getState());
+ FlightStatsRecorder::getInstance().update(ada.getState());
#ifdef HILSimulation
// useful only for hil testing
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index 326f26e32..878d41458 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -23,7 +23,9 @@
#include "FlightModeManager.h"
#include <Main/Actuators/Actuators.h>
+#include <Main/CanHandler/CanHandler.h>
#include <Main/Configs/FlightModeManagerConfig.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Main/Sensors/Sensors.h>
#include <common/events/Events.h>
#include <drivers/timer/TimestampTimer.h>
@@ -73,6 +75,7 @@ State FlightModeManager::state_on_ground(const Event& event)
}
case TMTC_RESET_BOARD:
{
+ CanHandler::getInstance().sendCamOffCommand();
Logger::getInstance().stop();
reboot();
return HANDLED;
@@ -126,8 +129,8 @@ State FlightModeManager::state_init_error(const Event& event)
{
case EV_ENTRY:
{
- Actuators::getInstance().buzzerError();
logStatus(FlightModeManagerState::INIT_ERROR);
+ Actuators::getInstance().buzzerError();
EventBroker::getInstance().post(FLIGHT_ERROR_DETECTED,
TOPIC_FLIGHT);
return HANDLED;
@@ -252,6 +255,7 @@ State FlightModeManager::state_disarmed(const Event& event)
case EV_ENTRY:
{
logStatus(FlightModeManagerState::DISARMED);
+
Actuators::getInstance().buzzerDisarmed();
Logger::getInstance().stop();
EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
@@ -338,6 +342,7 @@ State FlightModeManager::state_armed(const Event& event)
case EV_ENTRY:
{
logStatus(FlightModeManagerState::ARMED);
+
Actuators::getInstance().buzzerArmed();
Logger::getInstance().start();
EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT);
@@ -412,7 +417,7 @@ State FlightModeManager::state_flying(const Event& event)
}
case FLIGHT_MISSION_TIMEOUT:
{
- return transition(&FlightModeManager::state_landed);
+ return transition(&FlightModeManager::state_mission_ended);
}
default:
{
@@ -447,6 +452,8 @@ State FlightModeManager::state_ascending(const Event& event)
case TMTC_FORCE_APOGEE:
case TMTC_FORCE_EXPULSION:
{
+ FlightStatsRecorder::getInstance().setApogee(
+ Sensors::getInstance().getUbxGpsLastSample());
return transition(&FlightModeManager::state_drogue_descent);
}
default:
@@ -535,12 +542,58 @@ State FlightModeManager::state_terminal_descent(const Event& event)
State FlightModeManager::state_landed(const Event& event)
{
+ static uint16_t landingTimeoutEventId = -1;
+
switch (event)
{
case EV_ENTRY:
{
logStatus(FlightModeManagerState::LANDED);
+ landingTimeoutEventId =
+ EventBroker::getInstance().postDelayed<LANDING_TIMEOUT>(
+ FLIGHT_LANDING_TIMEOUT, TOPIC_FLIGHT);
+
+ Actuators::getInstance().buzzerLanded();
+
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ EventBroker::getInstance().removeDelayed(landingTimeoutEventId);
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case FLIGHT_LANDING_TIMEOUT:
+ {
+ return transition(&FlightModeManager::state_mission_ended);
+ }
+ case TMTC_RESET_BOARD:
+ {
+ CanHandler::getInstance().sendCamOffCommand();
+ Logger::getInstance().stop();
+ reboot();
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+State FlightModeManager::state_mission_ended(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
Actuators::getInstance().buzzerLanded();
Logger::getInstance().stop();
@@ -560,6 +613,7 @@ State FlightModeManager::state_landed(const Event& event)
}
case TMTC_RESET_BOARD:
{
+ CanHandler::getInstance().sendCamOffCommand();
Logger::getInstance().stop();
reboot();
return HANDLED;
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index aa21d1566..1e9449fba 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -78,6 +78,9 @@ public:
/// The rocket is on the ground after the flight.
Boardcore::State state_landed(const Boardcore::Event& event);
+ /// The rocket ended the flight and closes the log.
+ Boardcore::State state_mission_ended(const Boardcore::Event& event);
+
private:
FlightModeManager();
~FlightModeManager();
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
index 9081054d7..f0fa03db5 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -43,7 +43,8 @@ enum class FlightModeManagerState : uint8_t
ASCENDING,
DROGUE_DESCENT,
TERMINAL_DESCENT,
- LANDED
+ LANDED,
+ MISSION_ENDED,
};
struct FlightModeManagerStatus
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index bd5d9bcf8..04f71543e 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -24,6 +24,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Configs/NASConfig.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
#include <algorithms/NAS/StateInitializer.h>
@@ -76,6 +77,7 @@ void NASController::update()
nas.correctPitot(pitotData.deltaP, pressureData.pressure);
Logger::getInstance().log(nas.getState());
+ FlightStatsRecorder::getInstance().update(nas.getState());
}
#ifdef HILSimulation
diff --git a/src/boards/Payload/Configs/FlightModeManagerConfig.h b/src/boards/Payload/Configs/FlightModeManagerConfig.h
index 95462eb05..d36b0601d 100644
--- a/src/boards/Payload/Configs/FlightModeManagerConfig.h
+++ b/src/boards/Payload/Configs/FlightModeManagerConfig.h
@@ -25,7 +25,8 @@
namespace Payload
{
-constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000;
constexpr unsigned int APOGEE_TIMEOUT = 20 * 1000;
+constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000;
+constexpr unsigned int LANDING_TIMEOUT = 2 * 60 * 1000;
} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
index 9253e42d9..8e2591c63 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -84,6 +84,7 @@ State FlightModeManager::state_on_ground(const Event& event)
}
case TMTC_RESET_BOARD:
{
+ Actuators::getInstance().camOff();
Logger::getInstance().stop();
reboot();
return HANDLED;
@@ -247,8 +248,8 @@ State FlightModeManager::state_disarmed(const Event& event)
logStatus(FlightModeManagerState::DISARMED);
Actuators::getInstance().ledDisarmed();
- Logger::getInstance().stop();
Actuators::getInstance().camOff();
+ Logger::getInstance().stop();
EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
return HANDLED;
@@ -313,11 +314,6 @@ State FlightModeManager::state_test_mode(const Event& event)
{
return HANDLED;
}
- case TMTC_FORCE_MAIN:
- {
- EventBroker::getInstance().post(DPL_CUT_DROGUE, TOPIC_DPL);
- return HANDLED;
- }
case TMTC_EXIT_TEST_MODE:
{
return transition(&FlightModeManager::state_disarmed);
@@ -338,8 +334,8 @@ State FlightModeManager::state_armed(const Event& event)
logStatus(FlightModeManagerState::ARMED);
Actuators::getInstance().ledArmed();
- Logger::getInstance().start();
Actuators::getInstance().camOn();
+ Logger::getInstance().start();
EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT);
return HANDLED;
@@ -407,9 +403,8 @@ State FlightModeManager::state_flying(const Event& event)
case FLIGHT_MISSION_TIMEOUT:
{
WingController::getInstance().stop();
- Actuators::getInstance().camOff();
- return transition(&FlightModeManager::state_landed);
+ return transition(&FlightModeManager::state_mission_ended);
}
default:
{
@@ -543,12 +538,57 @@ State FlightModeManager::state_terminal_descent(const Event& event)
State FlightModeManager::state_landed(const Event& event)
{
+ static uint16_t landingTimeoutEventId = -1;
+
switch (event)
{
case EV_ENTRY:
{
logStatus(FlightModeManagerState::LANDED);
+ landingTimeoutEventId =
+ EventBroker::getInstance().postDelayed<LANDING_TIMEOUT>(
+ FLIGHT_LANDING_TIMEOUT, TOPIC_FLIGHT);
+
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ EventBroker::getInstance().removeDelayed(landingTimeoutEventId);
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&FlightModeManager::state_top);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
+ case FLIGHT_LANDING_TIMEOUT:
+ {
+ return transition(&FlightModeManager::state_mission_ended);
+ }
+ case TMTC_RESET_BOARD:
+ {
+ Actuators::getInstance().camOff();
+ Logger::getInstance().stop();
+ reboot();
+ return HANDLED;
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+State FlightModeManager::state_mission_ended(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ Actuators::getInstance().camOff();
Logger::getInstance().stop();
return HANDLED;
@@ -567,6 +607,7 @@ State FlightModeManager::state_landed(const Event& event)
}
case TMTC_RESET_BOARD:
{
+ Actuators::getInstance().camOff();
Logger::getInstance().stop();
reboot();
return HANDLED;
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
index a79a4031b..f5bfaeef4 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
@@ -77,6 +77,9 @@ public:
/// The payload is on the ground after the flight.
Boardcore::State state_landed(const Boardcore::Event& event);
+ /// The rocket ended the flight and closes the log.
+ Boardcore::State state_mission_ended(const Boardcore::Event& event);
+
private:
FlightModeManager();
~FlightModeManager();
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h
index e46b8c158..f5ec50adc 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -43,7 +43,8 @@ enum class FlightModeManagerState : uint8_t
ASCENDING,
DROGUE_DESCENT,
TERMINAL_DESCENT,
- LANDED
+ LANDED,
+ MISSION_ENDED,
};
struct FlightModeManagerStatus
diff --git a/src/boards/common/events/Events.h b/src/boards/common/events/Events.h
index 297ccbdf9..f9a031a8e 100644
--- a/src/boards/common/events/Events.h
+++ b/src/boards/common/events/Events.h
@@ -59,6 +59,7 @@ enum Events : uint8_t
FLIGHT_DPL_ALT_DETECTED,
FLIGHT_ERROR_DETECTED,
FLIGHT_LANDING_DETECTED,
+ FLIGHT_LANDING_TIMEOUT,
FLIGHT_LAUNCH_PIN_DETACHED,
FLIGHT_LIFTOFF,
FLIGHT_MISSION_TIMEOUT,
--
GitLab