diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index f4007eecdf2fe78ca1f5a9965191a26d466e8413..0a0905f194f5dad484d081d5f846a78577f50227 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -34,7 +34,7 @@ set(MAIN_COMPUTER
src/boards/Main/StateMachines/ADAController/ADAController.cpp
src/boards/Main/PinHandler/PinHandler.cpp
# src/boards/Main/StateMachines/ABKController/ABKController.cpp
- # src/boards/Main/StateMachines/MEAController/MEAController.cpp
+ src/boards/Main/StateMachines/MEAController/MEAController.cpp
src/boards/Main/StatsRecorder/StatsRecorder.cpp
)
diff --git a/src/boards/Main/BoardScheduler.h b/src/boards/Main/BoardScheduler.h
index 4ce9ade35c49a5adfa2d0c52072c546363768bef..11428599a702a4c921d51c74785ca71c7829253f 100644
--- a/src/boards/Main/BoardScheduler.h
+++ b/src/boards/Main/BoardScheduler.h
@@ -74,6 +74,8 @@ public:
Boardcore::TaskScheduler &getNasScheduler() { return nas; }
+ Boardcore::TaskScheduler &getMeaScheduler() { return nas; }
+
Boardcore::TaskScheduler &getAdaScheduler() { return ada; }
Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; }
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h
index a9437167ca965c0e8f600a03dcbdce29bafca104..648c5a0b38007e3e16b8dbf1ea211b29853c5208 100644
--- a/src/boards/Main/Configs/ADAConfig.h
+++ b/src/boards/Main/Configs/ADAConfig.h
@@ -37,8 +37,8 @@ namespace ADA
/* linter off */ using namespace Boardcore::Units::Frequency;
-constexpr Hertz SAMPLE_RATE = 50_hz;
-constexpr float SAMPLE_RATE_SECONDS = 0.02; // [s]
+constexpr Hertz UPDATE_RATE = 50_hz;
+constexpr float UPDATE_RATE_SECONDS = 0.02; // [s]
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
diff --git a/src/boards/Main/Configs/MEAConfig.h b/src/boards/Main/Configs/MEAConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fb6cfad5aed0ebccf4b30958a28598fd04c2549
--- /dev/null
+++ b/src/boards/Main/Configs/MEAConfig.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: 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/MEA/MEA.h>
+#include <units/Frequency.h>
+
+namespace Main
+{
+
+namespace Config
+{
+
+namespace MEA
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz UPDATE_RATE = 50_hz;
+
+constexpr unsigned int SHADOW_MODE_TIMEOUT = 4500; // [ms]
+
+constexpr float SHUTDOWN_APOGEE_TARGET = 3200; // [m]
+constexpr unsigned int SHUTDOWN_N_SAMPLES = 5;
+
+constexpr float SENSOR_NOISE_VARIANCE = 0.36f;
+constexpr float MODEL_NOISE_VARIANCE = 0.1f;
+constexpr float DEFAULT_INITIAL_ROCKET_MASS = 35.5920f;
+
+// Pressure threshold after which the kalman is updated
+constexpr float CC_PRESSURE_THRESHOLD = 1.f;
+
+constexpr Boardcore::Aeroutils::AerodynamicCoeff AERO_COEFF = {
+ .n000 = 0.596535425207973f,
+ .n100 = -1.81429600946981f,
+ .n200 = 8.47683559348987f,
+ .n300 = -23.1759370919254f,
+ .n400 = 35.8276525337534f,
+ .n500 = -29.2336913633527f,
+ .n600 = 9.84223649075812f};
+
+} // namespace MEA
+
+} // namespace Config
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
index f2802dd05cbccc9bbca5227250c134e384de166b..ace06075fabaea69f86753786481992e5fbc5135 100644
--- a/src/boards/Main/Configs/NASConfig.h
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -37,13 +37,14 @@ namespace NAS
/* linter off */ using namespace Boardcore::Units::Frequency;
-constexpr Hertz SAMPLE_RATE = 50_hz;
+constexpr Hertz UPDATE_RATE = 50_hz;
+constexpr float UPDATE_RATE_SECONDS = 0.02; // [s]
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
static const Boardcore::NASConfig CONFIG = {
- .T = 0.02, // [s]
+ .T = UPDATE_RATE_SECONDS,
.SIGMA_BETA = 0.0001,
.SIGMA_W = 0.3,
.SIGMA_ACC = 0.1,
@@ -65,6 +66,9 @@ static const Boardcore::NASConfig CONFIG = {
// Only use one out of every 50 samples (1 Hz)
constexpr int MAGNETOMETER_DECIMATE = 50;
+// Maximum allowed acceleration to correct with GPS
+constexpr float DISABLE_GPS_ACCELERATION = 34.0f; // [m/s^2]
+
// How much confidence (in m/s^2) to apply to the accelerometer to check if it
// is 1g
constexpr float ACCELERATION_1G_CONFIDENCE = 0.5;
diff --git a/src/boards/Main/Configs/SchedulerConfig.h b/src/boards/Main/Configs/SchedulerConfig.h
index f380166817feaacee862bbb6af0689f84fb99c02..8d3150dc5343025db42a1f2c5a9f9a005e050d7c 100644
--- a/src/boards/Main/Configs/SchedulerConfig.h
+++ b/src/boards/Main/Configs/SchedulerConfig.h
@@ -35,6 +35,8 @@ namespace Scheduler
// Used for NAS related activities (state machines/scheduler)
static const miosix::Priority NAS_PRIORITY = miosix::PRIORITY_MAX - 1;
+// Used for NAS related activities (state machines/scheduler)
+static const miosix::Priority MEA_PRIORITY = miosix::PRIORITY_MAX - 1;
// Used for ADA related activities (state machines/scheduler)
static const miosix::Priority ADA_PRIORITY = miosix::PRIORITY_MAX - 1;
// Used for Sensors TaskScheduler
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index 4a0304c250a368f6035ebf842f50d1638506871a..1c71d6a5c8bf23f3d2f8d66453bc6c56a458d630 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -532,6 +532,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
Actuators* actuators = getModule<Actuators>();
ADAController* ada = getModule<ADAController>();
NASController* nas = getModule<NASController>();
+ MEAController* mea = getModule<MEAController>();
auto pressDigi = sensors->getLPS22DFLastSample();
auto imu = sensors->getIMULastSample();
@@ -541,12 +542,13 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
auto pressDpl = sensors->getDplBayPressureLastSample();
auto adaState = ada->getADAState();
auto nasState = nas->getNASState();
+ auto meaState = mea->getMEAState();
tm.timestamp = TimestampTimer::getTimestamp();
tm.airspeed_pitot = -1.0f; // TODO
- tm.mea_mass = -1.0f; // TODO
- tm.mea_apogee = -1.0f; // TODO
+ tm.mea_mass = meaState.estimatedMass;
+ tm.mea_apogee = meaState.estimatedApogee;
// Sensors
tm.pressure_digi = pressDigi.pressure;
@@ -618,6 +620,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
FlightModeManager* fmm = getModule<FlightModeManager>();
ADAController* ada = getModule<ADAController>();
NASController* nas = getModule<NASController>();
+ MEAController* mea = getModule<MEAController>();
Actuators* actuators = getModule<Actuators>();
StatsRecorder* recorder = getModule<StatsRecorder>();
@@ -664,7 +667,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.fmm_state = static_cast<uint8_t>(fmm->getState());
tm.abk_state = 255; // TODO
tm.nas_state = static_cast<uint8_t>(nas->getState());
- tm.mea_state = 255; // TODO
+ tm.mea_state = static_cast<uint8_t>(mea->getState());
// Actuators
tm.exp_angle =
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
index 46fb7b9ab7c2b9217d2bafeb2ecdc6cd2187cf5a..e107dedc9cd2ff7b01a35dd8fd281c94730a66e3 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -31,6 +31,7 @@
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/ADAController/ADAController.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
#include <Main/StateMachines/NASController/NASController.h>
#include <Main/StatsRecorder/StatsRecorder.h>
#include <common/Mavlink.h>
@@ -45,10 +46,10 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
Config::Radio::MAV_OUT_QUEUE_SIZE,
Config::Radio::MAV_MAX_LENGTH>;
-class Radio
- : public Boardcore::InjectableWithDeps<
- Buses, BoardScheduler, Actuators, PinHandler, CanHandler, Sensors,
- FlightModeManager, ADAController, NASController, StatsRecorder>
+class Radio : public Boardcore::InjectableWithDeps<
+ Buses, BoardScheduler, Actuators, PinHandler, CanHandler,
+ Sensors, FlightModeManager, ADAController, NASController,
+ MEAController, StatsRecorder>
{
public:
Radio() {}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index f116bd29dc1a2bcb6eeb64780f1b02c606189c1d..1c781cfecadfa8395cf658723422dda29157e790 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -49,8 +49,8 @@ ADA::KalmanFilter::KalmanConfig computeADAKalmanConfig(float refPressure)
// clang-format off
F = ADA::KalmanFilter::MatrixNN({
- {1.0, Config::ADA::SAMPLE_RATE_SECONDS, 0.5f * Config::ADA::SAMPLE_RATE_SECONDS * Config::ADA::SAMPLE_RATE_SECONDS},
- {0.0, 1.0, Config::ADA::SAMPLE_RATE_SECONDS },
+ {1.0, Config::ADA::UPDATE_RATE_SECONDS, 0.5f * Config::ADA::UPDATE_RATE_SECONDS * Config::ADA::UPDATE_RATE_SECONDS},
+ {0.0, 1.0, Config::ADA::UPDATE_RATE_SECONDS },
{0.0, 0.0, 1.0 }});
H = {1.0, 0.0, 0.0};
@@ -91,7 +91,7 @@ bool ADAController::start()
TaskScheduler& scheduler = getModule<BoardScheduler>()->getAdaScheduler();
uint8_t result =
- scheduler.addTask([this]() { update(); }, Config::ADA::SAMPLE_RATE);
+ scheduler.addTask([this]() { update(); }, Config::ADA::UPDATE_RATE);
if (result == 0)
{
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index a3cd2a16fa13916ed77505a2cb1a76eed4792cb6..35212d12a9782b0d0a9f751a4e95e2e87eb9f5cb 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -41,6 +41,7 @@ FlightModeManager::FlightModeManager()
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
EventBroker::getInstance().subscribe(this, TOPIC_ADA);
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+ EventBroker::getInstance().subscribe(this, TOPIC_MEA);
}
FlightModeManagerState FlightModeManager::getState() { return state; }
@@ -358,6 +359,7 @@ State FlightModeManager::state_test_mode(const Event& event)
EventBroker::getInstance().post(ADA_FORCE_START, TOPIC_ADA);
EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
+ EventBroker::getInstance().post(MEA_FORCE_START, TOPIC_MEA);
getModule<Sensors>()->resetMagCalibrator();
getModule<Sensors>()->enableMagCalibrator();
return HANDLED;
@@ -366,6 +368,7 @@ State FlightModeManager::state_test_mode(const Event& event)
{
EventBroker::getInstance().post(ADA_FORCE_STOP, TOPIC_ADA);
EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS);
+ EventBroker::getInstance().post(MEA_FORCE_STOP, TOPIC_MEA);
getModule<Sensors>()->disableMagCalibrator();
return HANDLED;
}
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40ea0458736f7dbf56b6d1627bd891afaf18e366
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
@@ -0,0 +1,378 @@
+/* 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 "MEAController.h"
+
+#include <Main/Configs/MEAConfig.h>
+#include <Main/Configs/SchedulerConfig.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <events/EventBroker.h>
+
+using namespace Main;
+using namespace Boardcore;
+using namespace Common;
+using namespace miosix;
+using namespace Eigen;
+
+MEA::Config computeMEAConfig()
+{
+ MEA::Config config;
+
+ // clang-format off
+ config.F = Matrix<float, 3, 3>({
+ {1.435871191228868, -0.469001276508780, 0.f},
+ {1.f, 0.f, 0.f},
+ {-0.002045309260755, 0.001867496708935, 1.f}});
+ config.Q = Matrix<float, 3, 3>::Identity() * Config::MEA::MODEL_NOISE_VARIANCE;
+ config.G = Matrix<float, 3, 1>{{4}, {0}, {0}};
+
+ config.baroH = {1.780138883879285,-1.625379384370081,0.f};
+ config.baroR = Config::MEA::SENSOR_NOISE_VARIANCE;
+
+ config.P = Matrix<float, 3, 3>::Zero();
+ config.initialMass = Config::MEA::DEFAULT_INITIAL_ROCKET_MASS;
+
+ // For now, never trigger accel correction
+ config.accelThresh = 999999.0f;
+ config.speedThresh = 999999.0f;
+
+ // TODO: Update these
+ config.Kt = -1.0f;
+ config.alpha = -1.0f;
+ config.c = -1.0f;
+
+ config.coeffs = Config::MEA::AERO_COEFF;
+
+ // Rockets diameter
+ // TODO: De-hardcode this
+ float d = 0.15f;
+ config.crossSection = Constants::PI * (d / 2) * (d / 2);
+
+ // TODO: Update these
+ config.ae = -1.0f;
+ config.p0 = -1.0f;
+ // clang-format on
+
+ return config;
+}
+
+MEAController::MEAController()
+ : FSM{&MEAController::state_init, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ Config::Scheduler::MEA_PRIORITY},
+ mea{computeMEAConfig()}
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_MEA);
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+bool MEAController::start()
+{
+ TaskScheduler& scheduler = getModule<BoardScheduler>()->getMeaScheduler();
+
+ uint8_t result =
+ scheduler.addTask([this]() { update(); }, Config::MEA::UPDATE_RATE);
+
+ if (result == 0)
+ {
+ LOG_ERR(logger, "Failed to add MEA update task");
+ return false;
+ }
+
+ if (!FSM::start())
+ {
+ LOG_ERR(logger, "Failed to start MEA FSM");
+ return false;
+ }
+
+ return true;
+}
+
+MEAState MEAController::getMEAState()
+{
+ Lock<FastMutex> lock{meaMutex};
+ return mea.getState();
+}
+
+MEAControllerState MEAController::getState() { return state; }
+
+void MEAController::update()
+{
+ MEAControllerState curState = state;
+
+ // Lock MEA for the whole duration of the update
+ Lock<FastMutex> lock{meaMutex};
+
+ if (curState == MEAControllerState::ARMED ||
+ curState == MEAControllerState::SHADOW_MODE ||
+ curState == MEAControllerState::ACTIVE ||
+ curState == MEAControllerState::ACTIVE_UNPOWERED)
+ {
+ // Perform updates only during this phases
+
+ PressureData baro = getModule<Sensors>()->getCanCCPressLastSample();
+ IMUData imu = getModule<Sensors>()->getIMULastSample();
+ NASState nas = getModule<NASController>()->getNASState();
+ ReferenceValues reference =
+ getModule<NASController>()->getReferenceValues();
+
+ // TODO: Is this even correct?
+ float aperture =
+ getModule<Actuators>()->isCanServoOpen(ServosList::MAIN_VALVE)
+ ? 1.0f
+ : 0.0f;
+
+ if (baro.pressure > Config::MEA::CC_PRESSURE_THRESHOLD)
+ {
+ MEA::Step step{aperture};
+
+ if (baro.pressureTimestamp > lastBaroTimestamp)
+ {
+ step.withCCPressure(baro);
+ }
+
+ if (imu.accelerationTimestamp > lastAccTimestamp)
+ {
+ step.withAcceleration(imu);
+ }
+
+ if (nas.timestamp > lastNasTimestamp)
+ {
+ step.withSpeedAndAlt(-nas.vd, reference.refAltitude - nas.d);
+ }
+
+ mea.update(step);
+
+ MEAState state = mea.getState();
+
+ // Run detections
+ if (curState == MEAControllerState::SHADOW_MODE ||
+ curState == MEAControllerState::ACTIVE ||
+ curState == MEAControllerState::ACTIVE_UNPOWERED)
+ {
+ if (state.estimatedApogee > Config::MEA::SHUTDOWN_APOGEE_TARGET)
+ {
+ detectedShutdowns++;
+ }
+ else
+ {
+ detectedShutdowns = 0;
+ }
+
+ if (curState != MEAControllerState::SHADOW_MODE &&
+ curState == MEAControllerState::ACTIVE_UNPOWERED)
+ {
+ // DO NOT THROW EVENTS IN SHADOW_MODE!
+ if (detectedShutdowns > Config::MEA::SHUTDOWN_N_SAMPLES)
+ {
+ EventBroker::getInstance().post(MEA_SHUTDOWN_DETECTED,
+ TOPIC_MEA);
+ }
+ }
+ }
+
+ lastBaroTimestamp = baro.pressureTimestamp;
+ lastAccTimestamp = imu.accelerationTimestamp;
+ lastNasTimestamp = nas.timestamp;
+
+ sdLogger.log(state);
+ }
+ }
+}
+
+void MEAController::state_init(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::INIT);
+
+ // Immediately transition to ready
+ transition(&MEAController::state_ready);
+ break;
+ }
+ }
+}
+
+void MEAController::state_ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::READY);
+ break;
+ }
+
+ case MEA_FORCE_START:
+ case FLIGHT_ARMED:
+ {
+ transition(&MEAController::state_armed);
+ break;
+ }
+ }
+}
+
+void MEAController::state_armed(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::ARMED);
+ break;
+ }
+
+ case MEA_FORCE_STOP:
+ case FLIGHT_DISARMED:
+ {
+ transition(&MEAController::state_ready);
+ break;
+ }
+
+ case FLIGHT_LIFTOFF:
+ {
+ transition(&MEAController::state_shadow_mode);
+ break;
+ }
+ }
+}
+
+void MEAController::state_shadow_mode(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::SHADOW_MODE);
+
+ shadowModeTimeoutEvent = EventBroker::getInstance().postDelayed(
+ MEA_SHADOW_MODE_TIMEOUT, TOPIC_MEA,
+ Config::MEA::SHADOW_MODE_TIMEOUT);
+ break;
+ }
+
+ case EV_EXIT:
+ {
+ EventBroker::getInstance().removeDelayed(shadowModeTimeoutEvent);
+ break;
+ }
+
+ case MEA_FORCE_STOP:
+ {
+ transition(&MEAController::state_ready);
+ break;
+ }
+
+ case MEA_SHADOW_MODE_TIMEOUT:
+ {
+ transition(&MEAController::state_active);
+ break;
+ }
+
+ case FLIGHT_APOGEE_DETECTED:
+ {
+ transition(&MEAController::state_active_unpowered);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&MEAController::state_end);
+ break;
+ }
+ }
+}
+
+void MEAController::state_active(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::ACTIVE);
+ break;
+ }
+
+ case MEA_FORCE_STOP:
+ {
+ transition(&MEAController::state_ready);
+ break;
+ }
+
+ case FLIGHT_APOGEE_DETECTED:
+ {
+ transition(&MEAController::state_active_unpowered);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&MEAController::state_end);
+ break;
+ }
+ }
+}
+
+void MEAController::state_active_unpowered(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::ACTIVE_UNPOWERED);
+ break;
+ }
+
+ case MEA_FORCE_STOP:
+ {
+ transition(&MEAController::state_ready);
+ break;
+ }
+
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&MEAController::state_end);
+ break;
+ }
+ }
+}
+
+void MEAController::state_end(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(MEAControllerState::END);
+ break;
+ }
+ }
+}
+
+void MEAController::updateAndLogStatus(MEAControllerState state)
+{
+ this->state = state;
+ MEAControllerStatus data = {TimestampTimer::getTimestamp(), state};
+ sdLogger.log(data);
+}
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main/StateMachines/MEAController/MEAController.h
new file mode 100644
index 0000000000000000000000000000000000000000..54500cd2147d49facb08ed597e790ab23a5674f4
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.h
@@ -0,0 +1,83 @@
+/* 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 <Main/Actuators/Actuators.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/MEAController/MEAControllerData.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <algorithms/MEA/MEA.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class MEAController
+ : public Boardcore::FSM<MEAController>,
+ public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, Sensors,
+ NASController>
+{
+public:
+ MEAController();
+
+ [[nodiscard]] bool start() override;
+
+ Boardcore::MEAState getMEAState();
+
+ MEAControllerState getState();
+
+private:
+ void update();
+
+ // FSM states
+ void state_init(const Boardcore::Event& event);
+ void state_ready(const Boardcore::Event& event);
+ void state_armed(const Boardcore::Event& event);
+ void state_shadow_mode(const Boardcore::Event& event);
+ void state_active(const Boardcore::Event& event);
+ void state_active_unpowered(const Boardcore::Event& event);
+ void state_end(const Boardcore::Event& event);
+
+ void updateAndLogStatus(MEAControllerState state);
+
+ std::atomic<MEAControllerState> state{MEAControllerState::INIT};
+
+ Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("mea");
+
+ uint16_t shadowModeTimeoutEvent = 0;
+
+ miosix::FastMutex meaMutex;
+ Boardcore::MEA mea;
+
+ uint64_t lastAccTimestamp = 0;
+ uint64_t lastBaroTimestamp = 0;
+ uint64_t lastNasTimestamp = 0;
+ unsigned int detectedShutdowns = 0;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/MEAController/MEAControllerData.h b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..b60e8e1ffcd9b6fa0d1f9e3b53b72026e5e494ce
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h
@@ -0,0 +1,56 @@
+/* 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 <cstdint>
+#include <ostream>
+#include <string>
+
+namespace Main
+{
+
+enum class MEAControllerState : uint8_t
+{
+ INIT = 0,
+ READY,
+ ARMED,
+ SHADOW_MODE,
+ ACTIVE,
+ ACTIVE_UNPOWERED,
+ END
+};
+
+struct MEAControllerStatus
+{
+ uint64_t timestamp = 0;
+ MEAControllerState state = MEAControllerState::INIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(state) << "\n";
+ }
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index 6361f0548bce9c9b8c23761182813d800bb88f39..a50d4b7ba84e16c2bb0f2007b80c0870b229564b 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -66,7 +66,7 @@ bool NASController::start()
TaskScheduler& scheduler = getModule<BoardScheduler>()->getNasScheduler();
size_t result =
- scheduler.addTask([this]() { update(); }, Config::NAS::SAMPLE_RATE);
+ scheduler.addTask([this]() { update(); }, Config::NAS::UPDATE_RATE);
if (result == 0)
{
@@ -111,6 +111,10 @@ void NASController::update()
UBXGPSData gps = sensors->getUBXGPSLastSample();
PressureData baro = sensors->getAtmosPressureLastSample();
+ // Calculate acceleration
+ Vector3f acc = static_cast<AccelerometerData>(imu);
+ float accLength = acc.norm();
+
// Perform initial NAS prediction
// TODO: What about stale data?
nas.predictGyro(imu);
@@ -128,7 +132,8 @@ void NASController::update()
magDecimateCount++;
}
- if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3)
+ if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
+ accLength < Config::NAS::DISABLE_GPS_ACCELERATION)
{
nas.correctGPS(gps);
}
@@ -147,9 +152,6 @@ void NASController::update()
}
// Check if the accelerometer is measuring 1g
- Vector3f acc = static_cast<AccelerometerData>(imu);
- float accLength = acc.norm();
-
if (accLength <
(Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
accLength >
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
index 6b2f32fe365a52a4744347171679e938788bdfea..042c8f927a8e30035b9a5953646eed295d265585 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -65,7 +65,7 @@ private:
std::atomic<NASControllerState> state{NASControllerState::INIT};
Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
- Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ada");
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("nas");
miosix::FastMutex nasMutex;
Boardcore::NAS nas;
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index afbe0e75c52fb4eee1b23854c1dde0353c6b17cb..db4900fe5aba361704eab00801887567a1a5e6f0 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -29,6 +29,7 @@
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/ADAController/ADAController.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
#include <Main/StateMachines/NASController/NASController.h>
#include <Main/StatsRecorder/StatsRecorder.h>
#include <actuators/Servo/Servo.h>
@@ -61,6 +62,7 @@ int main()
FlightModeManager *fmm = new FlightModeManager();
ADAController *ada = new ADAController();
NASController *nas = new NASController();
+ MEAController *mea = new MEAController();
StatsRecorder *recorder = new StatsRecorder();
// Insert modules
@@ -74,6 +76,7 @@ int main()
manager.insert<FlightModeManager>(fmm) &&
manager.insert<ADAController>(ada) &&
manager.insert<NASController>(nas) &&
+ manager.insert<MEAController>(mea) &&
manager.insert<StatsRecorder>(recorder) && manager.inject();
manager.graphviz(std::cout);
@@ -156,6 +159,12 @@ int main()
std::cout << "Error failed to start NASController" << std::endl;
}
+ if (!mea->start())
+ {
+ initResult = false;
+ std::cout << "Error failed to start MEAController" << std::endl;
+ }
+
if (!fmm->start())
{
initResult = false;