diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 1fdb17226ea6765da0b63743c7d3852d9c03a0c1..f4007eecdf2fe78ca1f5a9965191a26d466e8413 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -30,15 +30,12 @@ set(MAIN_COMPUTER
src/boards/Main/CanHandler/CanHandler.cpp
src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
src/boards/Main/Actuators/Actuators.cpp
- # src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
src/boards/Main/StateMachines/NASController/NASController.cpp
src/boards/Main/StateMachines/ADAController/ADAController.cpp
src/boards/Main/PinHandler/PinHandler.cpp
- # src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
# src/boards/Main/StateMachines/ABKController/ABKController.cpp
# src/boards/Main/StateMachines/MEAController/MEAController.cpp
- # src/boards/Main/StateMachines/Deployment/Deployment.cpp
- # src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
+ src/boards/Main/StatsRecorder/StatsRecorder.cpp
)
set(MOTOR_SOURCES
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index b91f582bf1b84ebd8cec7766e8de283d6546a6e5..0d6e99e852ca84ae1861e745a58c40a18e84f52b 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -412,7 +412,11 @@ void Sensors::h3lis331dlInit()
void Sensors::h3lis331dlCallback()
{
- Logger::getInstance().log(getH3LIS331DLLastSample());
+ auto sample = getH3LIS331DLLastSample();
+
+ // Also update StatsRecorder
+ getModule<StatsRecorder>()->updateAcc(sample);
+ Logger::getInstance().log(sample);
}
void Sensors::lis2mdlInit()
@@ -598,8 +602,10 @@ void Sensors::dplBayPressureInit()
void Sensors::dplBayPressureCallback()
{
- Logger::getInstance().log(
- DplBayPressureData{getDplBayPressureLastSample()});
+ auto sample = getDplBayPressureLastSample();
+
+ getModule<StatsRecorder>()->updateDplPressure(sample);
+ Logger::getInstance().log(DplBayPressureData{sample});
}
void Sensors::rotatedImuInit()
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index 11476a8dac7f45cf9bde975d961112b537e8f9ec..046b9bcea1fd3738fa19c412f327c0b7642c07ae 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -24,6 +24,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Buses.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
#include <diagnostic/PrintLogger.h>
#include <drivers/adc/InternalADC.h>
#include <scheduler/TaskScheduler.h>
@@ -46,7 +47,8 @@
namespace Main
{
-class Sensors : public Boardcore::InjectableWithDeps<Buses, BoardScheduler>
+class Sensors
+ : public Boardcore::InjectableWithDeps<Buses, BoardScheduler, StatsRecorder>
{
public:
Sensors() {}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index bbb5a7be71f99c6ba3238b6a6f548dc3fd5f7b93..9cdcc2f36f946d2839093634b4ea6f093bf3b2d5 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -177,6 +177,13 @@ void ADAController::update()
// DO NOT THROW EVENTS IN SHADOW_MODE!
if (detectedApogees > Config::ADA::APOGEE_N_SAMPLES)
{
+ auto gps = getModule<Sensors>()->getUBXGPSLastSample();
+
+ // Notify stats recorder
+ getModule<StatsRecorder>()->apogeeDetected(
+ TimestampTimer::getTimestamp(), gps.latitude, gps.longitude,
+ ada.getState().mslAltitude);
+
EventBroker::getInstance().post(ADA_APOGEE_DETECTED, TOPIC_ADA);
}
}
@@ -195,7 +202,10 @@ void ADAController::update()
if (detectedDeployments > Config::ADA::DEPLOYMENT_N_SAMPLES)
{
- // TODO(davide.mor): Rename this event
+ // Notify stats recorder
+ getModule<StatsRecorder>()->deploymentDetected(
+ TimestampTimer::getTimestamp(), ada.getState().mslAltitude);
+
EventBroker::getInstance().post(ADA_DEPLOY_ALTITUDE_DETECTED,
TOPIC_ADA);
}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index e26c7da7d6dd38060dcf670b18a59b36731918f5..5b5e12334c023dbc6c75f70346e5aeffa7c8e90a 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -25,6 +25,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/ADAController/ADAControllerData.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
#include <algorithms/ADA/ADA.h>
#include <events/FSM.h>
#include <utils/DependencyManager/DependencyManager.h>
@@ -33,7 +34,8 @@ namespace Main
{
class ADAController
- : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>,
+ : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+ StatsRecorder>,
public Boardcore::FSM<ADAController>
{
public:
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index ac2909fad6513bf5fad340b76f620b80054b7e86..15c4d775bf11bee09f2036c1c4e6d6afa18c0313 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -350,6 +350,10 @@ State FlightModeManager::state_test_mode(const Event& event)
case EV_ENTRY:
{
updateAndLogStatus(FlightModeManagerState::TEST_MODE);
+
+ // Reset all stats
+ getModule<StatsRecorder>()->reset();
+
EventBroker::getInstance().post(ADA_FORCE_START, TOPIC_ADA);
EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
getModule<Sensors>()->resetMagCalibrator();
@@ -406,6 +410,9 @@ State FlightModeManager::state_armed(const Event& event)
{
updateAndLogStatus(FlightModeManagerState::ARMED);
+ // Reset all stats
+ getModule<StatsRecorder>()->reset();
+
Logger::getInstance().stop();
Logger::getInstance().start();
Logger::getInstance().resetStats();
@@ -441,6 +448,8 @@ State FlightModeManager::state_armed(const Event& event)
case TMTC_FORCE_LAUNCH:
case FLIGHT_LAUNCH_PIN_DETACHED:
{
+ getModule<StatsRecorder>()->liftoffDetected(
+ TimestampTimer::getTimestamp());
getModule<CanHandler>()->sendEvent(CanConfig::EventId::LIFTOFF);
return transition(&FlightModeManager::state_flying);
}
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index 524fa91518587ea713fc0f620f7a6a0d6a53e875..ffe151608cdf94c750da6b800de1ef4980b8c099 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -25,6 +25,7 @@
#include <Main/Actuators/Actuators.h>
#include <Main/CanHandler/CanHandler.h>
#include <Main/Sensors/Sensors.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
#include <events/EventBroker.h>
#include <events/HSM.h>
#include <utils/DependencyManager/DependencyManager.h>
@@ -35,7 +36,8 @@ namespace Main
{
class FlightModeManager
- : public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler>,
+ : public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler,
+ StatsRecorder>,
public Boardcore::HSM<FlightModeManager>
{
public:
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index e377dd3deaad81a6242a1124a80ffbdb850e38d1..5aba8e194fe61aa7908bbc77895615a92e91ebb3 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -175,7 +175,10 @@ void NASController::update()
lastGpsTimestamp = gps.gpsTimestamp;
lastBaroTimestamp = baro.pressureTimestamp;
- sdLogger.log(nas.getState());
+ auto state = nas.getState();
+
+ getModule<StatsRecorder>()->updateNas(state);
+ sdLogger.log(state);
}
}
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
index de145f7d90ec906bb6bb8e3527c5cddc55cf3bd2..6b2f32fe365a52a4744347171679e938788bdfea 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -25,6 +25,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/NASController/NASControllerData.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
#include <algorithms/NAS/NAS.h>
#include <diagnostic/PrintLogger.h>
#include <events/FSM.h>
@@ -35,7 +36,8 @@ namespace Main
class NASController
: public Boardcore::FSM<NASController>,
- public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>
+ public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+ StatsRecorder>
{
public:
NASController();
diff --git a/src/boards/Main/StatsRecorder/StatsRecorder.cpp b/src/boards/Main/StatsRecorder/StatsRecorder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..424aa6004ac19942e387ded47f350ad94b44b10b
--- /dev/null
+++ b/src/boards/Main/StatsRecorder/StatsRecorder.cpp
@@ -0,0 +1,145 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "StatsRecorder.h"
+
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+
+using namespace Boardcore;
+using namespace Main;
+using namespace miosix;
+using namespace Eigen;
+
+StatsRecorder::StatsRecorder() {}
+
+void StatsRecorder::reset()
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats = Stats{};
+}
+
+StatsRecorder::Stats StatsRecorder::getStats()
+{
+ Lock<FastMutex> lock{statsMutex};
+ return stats;
+}
+
+void StatsRecorder::liftoffDetected(uint64_t ts)
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats.liftoffTs = ts;
+}
+
+void StatsRecorder::apogeeDetected(uint64_t ts, float lat, float lon, float alt)
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats.apogeeTs = ts;
+ stats.apogeeLat = lat;
+ stats.apogeeLon = lon;
+ stats.apogeeAlt = alt;
+}
+
+void StatsRecorder::deploymentDetected(uint64_t ts, float alt)
+{
+ Lock<FastMutex> lock{statsMutex};
+ stats.dplTs = ts;
+ stats.dplAlt = alt;
+}
+
+void StatsRecorder::updateAcc(const AccelerometerData &data)
+{
+ auto state = getModule<FlightModeManager>()->getState();
+
+ float length = static_cast<Vector3f>(data).norm();
+ Lock<FastMutex> lock{statsMutex};
+
+ if (state == FlightModeManagerState::POWERED_ASCENT ||
+ state == FlightModeManagerState::ARMED)
+ {
+ // Record this event only during liftoff
+ if (length > stats.liftoffMaxAcc)
+ {
+ stats.liftoffMaxAcc = length;
+ stats.liftoffMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+ else if (state == FlightModeManagerState::DROGUE_DESCENT)
+ {
+ // Record this event only during drogue deployment
+ if (length > stats.apogeeMaxAcc)
+ {
+ stats.apogeeMaxAcc = length;
+ stats.apogeeMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+ else if (state == FlightModeManagerState::TERMINAL_DESCENT)
+ {
+ // Record this event only during main deployment
+ if (length > stats.dplMaxAcc)
+ {
+ stats.dplMaxAcc = length;
+ stats.dplMaxAccTs = data.accelerationTimestamp;
+ }
+ }
+}
+
+void StatsRecorder::updateNas(const NASState &data)
+{
+ auto state = getModule<FlightModeManager>()->getState();
+
+ Lock<FastMutex> lock{statsMutex};
+ if (state == FlightModeManagerState::POWERED_ASCENT ||
+ state == FlightModeManagerState::UNPOWERED_ASCENT ||
+ state == FlightModeManagerState::DROGUE_DESCENT ||
+ state == FlightModeManagerState::TERMINAL_DESCENT)
+ {
+ // Record this event only during flight
+ float speed = Vector3f{data.vn, data.vd, data.ve}.norm();
+ float alt = -data.d;
+
+ if (speed > stats.maxSpeed)
+ {
+ stats.maxSpeed = speed;
+ stats.maxSpeedAlt = alt;
+ stats.maxSpeedTs = data.timestamp;
+ }
+
+ // TODO: Update mach
+ }
+}
+
+void StatsRecorder::updateDplPressure(const PressureData &data)
+{
+ auto state = getModule<FlightModeManager>()->getState();
+
+ Lock<FastMutex> lock{statsMutex};
+ if (state == FlightModeManagerState::DROGUE_DESCENT ||
+ state == FlightModeManagerState::TERMINAL_DESCENT)
+ {
+ // Record this event only in drogue or terminal descent
+ if (data.pressure > stats.maxDplPressure)
+ {
+ stats.maxDplPressure = data.pressure;
+ stats.maxDplPressureTs = data.pressureTimestamp;
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/boards/Main/StatsRecorder/StatsRecorder.h b/src/boards/Main/StatsRecorder/StatsRecorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c960b361551bcbc6a1c98829a28c7dd71fc71e9
--- /dev/null
+++ b/src/boards/Main/StatsRecorder/StatsRecorder.h
@@ -0,0 +1,99 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/ADA/ADAData.h>
+#include <algorithms/NAS/NASState.h>
+#include <miosix.h>
+#include <sensors/SensorData.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class FlightModeManager;
+
+class StatsRecorder : public Boardcore::InjectableWithDeps<FlightModeManager>
+{
+public:
+ struct Stats
+ {
+ // Liftoff
+ uint64_t liftoffTs = 0;
+
+ // Maximum acceleration during liftoff
+ uint64_t liftoffMaxAccTs = 0;
+ float liftoffMaxAcc = 0.0f;
+
+ // Maximum vertical speed
+ uint64_t maxSpeedTs = 0;
+ float maxSpeed = 0.0f;
+ float maxSpeedAlt = 0.0f;
+
+ // Max mach
+ uint64_t maxMachTs = 0;
+ float maxMach = 0.0f;
+
+ // Apogee
+ uint64_t apogeeTs = 0;
+ float apogeeLat = 0.0f;
+ float apogeeLon = 0.0f;
+ float apogeeAlt = 0.0f;
+
+ // Maximum acceleration after apogee
+ uint64_t apogeeMaxAccTs = 0;
+ float apogeeMaxAcc = 0.0f;
+
+ // Deployment
+ uint64_t dplTs = 0;
+ float dplAlt = 0.0f;
+
+ // Maximum acceleration after deployment
+ uint64_t dplMaxAccTs = 0;
+ float dplMaxAcc = 0.0f;
+
+ // Maximum deployment pressure
+ uint64_t maxDplPressureTs = 0;
+ float maxDplPressure = 0.0f;
+ };
+
+ StatsRecorder();
+
+ void reset();
+
+ Stats getStats();
+
+ void liftoffDetected(uint64_t ts);
+ void apogeeDetected(uint64_t ts, float lat, float lon, float alt);
+ void deploymentDetected(uint64_t ts, float alt);
+
+ void updateAcc(const Boardcore::AccelerometerData &data);
+ void updateNas(const Boardcore::NASState &data);
+ void updateDplPressure(const Boardcore::PressureData &data);
+
+private:
+ miosix::FastMutex statsMutex;
+ Stats stats;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 58bf5cd44819622d38b13bd66d563df71eb50238..8b23563284415f8cbc48a434094c7b61a1a6e4b2 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -30,6 +30,7 @@
#include <Main/StateMachines/ADAController/ADAController.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
#include <actuators/Servo/Servo.h>
#include <drivers/timer/PWM.h>
#include <events/EventBroker.h>
@@ -52,26 +53,28 @@ int main()
Buses *buses = new Buses();
BoardScheduler *scheduler = new BoardScheduler();
- Actuators *actuators = new Actuators();
- Sensors *sensors = new Sensors();
- Radio *radio = new Radio();
- CanHandler *canHandler = new CanHandler();
- PinHandler *pinHandler = new PinHandler();
- FlightModeManager *fmm = new FlightModeManager();
- ADAController *ada = new ADAController();
- NASController *nas = new NASController();
+ Actuators *actuators = new Actuators();
+ Sensors *sensors = new Sensors();
+ Radio *radio = new Radio();
+ CanHandler *canHandler = new CanHandler();
+ PinHandler *pinHandler = new PinHandler();
+ FlightModeManager *fmm = new FlightModeManager();
+ ADAController *ada = new ADAController();
+ NASController *nas = new NASController();
+ StatsRecorder *recorder = new StatsRecorder();
// Insert modules
- bool initResult = manager.insert<Buses>(buses) &&
- manager.insert<BoardScheduler>(scheduler) &&
- manager.insert<Sensors>(sensors) &&
- manager.insert<Radio>(radio) &&
- manager.insert<Actuators>(actuators) &&
- manager.insert<CanHandler>(canHandler) &&
- manager.insert<PinHandler>(pinHandler) &&
- manager.insert<FlightModeManager>(fmm) &&
- manager.insert<ADAController>(ada) &&
- manager.insert<NASController>(nas) && manager.inject();
+ bool initResult =
+ manager.insert<Buses>(buses) &&
+ manager.insert<BoardScheduler>(scheduler) &&
+ manager.insert<Sensors>(sensors) && manager.insert<Radio>(radio) &&
+ manager.insert<Actuators>(actuators) &&
+ manager.insert<CanHandler>(canHandler) &&
+ manager.insert<PinHandler>(pinHandler) &&
+ manager.insert<FlightModeManager>(fmm) &&
+ manager.insert<ADAController>(ada) &&
+ manager.insert<NASController>(nas) &&
+ manager.insert<StatsRecorder>(recorder) && manager.inject();
manager.graphviz(std::cout);