From 2bf6d3a81145e51ea82d74dbc33263e55976398f Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 29 Jul 2024 18:44:09 +0200
Subject: [PATCH] [Main] Initial NAS implementation
---
cmake/dependencies.cmake | 2 +-
src/boards/Main/Configs/NASConfig.h | 67 ++++++
src/boards/Main/Radio/Radio.cpp | 17 +-
src/boards/Main/Radio/Radio.h | 4 +-
src/boards/Main/Sensors/Sensors.h | 2 +-
.../ADAController/ADAController.cpp | 45 +++-
.../ADAController/ADAController.h | 2 +
.../FlightModeManager/FlightModeManager.cpp | 11 +-
.../NASController/NASController.cpp | 196 ++++++++++++++++++
.../NASController/NASController.h | 66 ++++++
.../NASController/NASControllerData.h | 54 +++++
src/entrypoints/Main/main-entry.cpp | 12 +-
12 files changed, 459 insertions(+), 19 deletions(-)
create mode 100644 src/boards/Main/Configs/NASConfig.h
create mode 100644 src/boards/Main/StateMachines/NASController/NASController.cpp
create mode 100644 src/boards/Main/StateMachines/NASController/NASController.h
create mode 100644 src/boards/Main/StateMachines/NASController/NASControllerData.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 79c336a49..1fdb17226 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -26,12 +26,12 @@ set(OBSW_INCLUDE_DIRS
set(MAIN_COMPUTER
src/boards/Main/Sensors/Sensors.cpp
- # src/boards/Main/StateMachines/NASController/NASController.cpp
src/boards/Main/Radio/Radio.cpp
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
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
new file mode 100644
index 000000000..46a3ee52e
--- /dev/null
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -0,0 +1,67 @@
+/* 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/NAS/NASConfig.h>
+#include <units/Frequency.h>
+#include <common/ReferenceConfig.h>
+
+namespace Main
+{
+
+namespace Config
+{
+
+namespace NAS
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz SAMPLE_RATE = 50_hz;
+
+static const Boardcore::NASConfig CONFIG = {
+ .T = 0.02, // [s]
+ .SIGMA_BETA = 0.0001,
+ .SIGMA_W = 0.3,
+ .SIGMA_ACC = 0.1,
+ .SIGMA_MAG = 0.1,
+ .SIGMA_GPS = 10.0,
+ .SIGMA_BAR = 4.3,
+ .SIGMA_POS = 10.0,
+ .SIGMA_VEL = 10.0,
+ .SIGMA_PITOT = 10.0,
+ .P_POS = 1.0,
+ .P_POS_VERTICAL = 10.0,
+ .P_VEL = 1.0,
+ .P_VEL_VERTICAL = 10.0,
+ .P_ATT = 0.01,
+ .P_BIAS = 0.01,
+ .SATS_NUM = 6.0,
+ .NED_MAG = Common::ReferenceConfig::nedMag
+};
+
+}
+
+}
+
+}
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index 99d398a6e..87d8480d5 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -417,6 +417,9 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
Sensors* sensors = getModule<Sensors>();
PinHandler* pinHandler = getModule<PinHandler>();
+ FlightModeManager *fmm = getModule<FlightModeManager>();
+ ADAController *ada = getModule<ADAController>();
+ NASController *nas = getModule<NASController>();
auto pressDigi = sensors->getLPS22DFLastSample();
auto imu = sensors->getLSM6DSRXLastSample();
@@ -424,15 +427,18 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
auto gps = sensors->getUBXGPSLastSample();
auto pressStatic = sensors->getStaticPressure1LastSample();
auto pressDpl = sensors->getDplBayPressureLastSample();
+ auto adaState = ada->getADAState();
tm.timestamp = TimestampTimer::getTimestamp();
- tm.pressure_ada = -1.0f; // TODO
tm.pressure_digi = pressDigi.pressure;
tm.pressure_static = pressStatic.pressure;
tm.pressure_dpl = pressDpl.pressure;
+
+ tm.pressure_ada = adaState.x0;
+ tm.ada_vert_speed = adaState.verticalSpeed;
+
tm.airspeed_pitot = -1.0f; // TODO
tm.altitude_agl = -1.0f; // TODO
- tm.ada_vert_speed = -1.0f; // TODO
tm.mea_mass = -1.0f; // TODO
// Sensors
@@ -471,12 +477,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
sensors->getCamBatteryVoltageLastSample().voltage;
tm.temperature = pressDigi.temperature;
- tm.ada_state = 255; // TODO
- tm.fmm_state = static_cast<uint8_t>(
- getModule<FlightModeManager>()->getState());
+ tm.ada_state = static_cast<uint8_t>(ada->getState());
+ tm.fmm_state = static_cast<uint8_t>(fmm->getState());
tm.dpl_state = 255; // TODO
tm.abk_state = 255; // TODO
- tm.nas_state = 255; // TODO
+ tm.nas_state = static_cast<uint8_t>(nas->getState());
tm.mea_state = 255; // TODO
tm.pin_launch =
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
index fca5dd6dd..81616e2df 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -30,6 +30,8 @@
#include <Main/PinHandler/PinHandler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
#include <common/Mavlink.h>
#include <radio/MavlinkDriver/MavlinkDriver.h>
#include <radio/SX1278/SX1278Fsk.h>
@@ -45,7 +47,7 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
class Radio
: public Boardcore::InjectableWithDeps<Buses, BoardScheduler, Actuators,
PinHandler, CanHandler, Sensors,
- FlightModeManager>
+ FlightModeManager, ADAController, NASController>
{
public:
Radio() {}
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index bb8035185..59f71c323 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -87,7 +87,7 @@ public:
protected:
virtual bool postSensorCreationHook() { return true; }
- TaskScheduler &getSensorsScheduler();
+ Boardcore::TaskScheduler &getSensorsScheduler();
miosix::FastMutex canMutex;
Boardcore::PressureData canCCPressure;
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index 65c65ddcb..5999862d6 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -114,11 +114,15 @@ ADAState ADAController::getADAState()
return ada.getState();
}
+ADAControllerState ADAController::getState()
+{
+ return state;
+}
+
void ADAController::update()
{
PressureData baro = getModule<Sensors>()->getStaticPressure1LastSample();
- // TODO(davide.mor): What about testing?
ADAControllerState curState = state;
// Lock ADA for the whole duration of the update
@@ -225,6 +229,7 @@ void ADAController::state_init(const Event& event)
updateAndLogStatus(ADAControllerState::INIT);
break;
}
+
case ADA_CALIBRATE:
{
transition(&ADAController::state_calibrating);
@@ -240,10 +245,10 @@ void ADAController::state_calibrating(const Event& event)
case EV_ENTRY:
{
updateAndLogStatus(ADAControllerState::CALIBRATING);
-
calibrate();
break;
}
+
case ADA_READY:
{
transition(&ADAController::state_ready);
@@ -261,11 +266,14 @@ void ADAController::state_ready(const Event& event)
updateAndLogStatus(ADAControllerState::READY);
break;
}
+
case ADA_CALIBRATE:
{
transition(&ADAController::state_calibrating);
break;
}
+
+ case ADA_FORCE_START:
case FLIGHT_ARMED:
{
transition(&ADAController::state_armed);
@@ -283,11 +291,19 @@ void ADAController::state_armed(const Event& event)
updateAndLogStatus(ADAControllerState::ARMED);
break;
}
+
+ case ADA_FORCE_STOP:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+
case FLIGHT_DISARMED:
{
transition(&ADAController::state_ready);
break;
}
+
case FLIGHT_LIFTOFF:
{
transition(&ADAController::state_shadow_mode);
@@ -316,6 +332,12 @@ void ADAController::state_shadow_mode(const Event& event)
break;
}
+ case ADA_FORCE_STOP:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+
case ADA_SHADOW_MODE_TIMEOUT:
{
transition(&ADAController::state_active_ascent);
@@ -340,6 +362,12 @@ void ADAController::state_active_ascent(const Event& event)
break;
}
+ case ADA_FORCE_STOP:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+
case FLIGHT_APOGEE_DETECTED:
{
transition(&ADAController::state_active_drogue_descent);
@@ -364,6 +392,12 @@ void ADAController::state_active_drogue_descent(const Event& event)
break;
}
+ case ADA_FORCE_STOP:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+
case FLIGHT_DPL_ALT_DETECTED:
{
transition(&ADAController::state_active_terminal_descent);
@@ -387,6 +421,13 @@ void ADAController::state_active_terminal_descent(const Event& event)
updateAndLogStatus(ADAControllerState::ACTIVE_TERMINAL_DESCENT);
break;
}
+
+ case ADA_FORCE_STOP:
+ {
+ transition(&ADAController::state_ready);
+ break;
+ }
+
case FLIGHT_LANDING_DETECTED:
{
transition(&ADAController::state_end);
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index 5d559a858..6af46ed98 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -43,6 +43,8 @@ public:
Boardcore::ADAState getADAState();
+ ADAControllerState getState();
+
private:
void update();
void calibrate();
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index d350b76fe..4b6ae9b82 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -235,11 +235,6 @@ State FlightModeManager::state_calibrate_algorithms(const Event& event)
EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS);
EventBroker::getInstance().post(ADA_CALIBRATE, TOPIC_ADA);
- // Quick hack to make the state machine go forward
- Thread::sleep(2000);
- EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
- EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
-
return HANDLED;
}
case EV_EXIT:
@@ -354,12 +349,14 @@ State FlightModeManager::state_test_mode(const Event& event)
case EV_ENTRY:
{
updateAndLogStatus(FlightModeManagerState::TEST_MODE);
- // TODO(davide.mor): Start algorithms
+ EventBroker::getInstance().post(ADA_FORCE_START, TOPIC_ADA);
+ EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
return HANDLED;
}
case EV_EXIT:
{
- // TODO(davide.mor): Stop algorithms
+ EventBroker::getInstance().post(ADA_FORCE_STOP, TOPIC_ADA);
+ EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS);
return HANDLED;
}
case EV_EMPTY:
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
new file mode 100644
index 000000000..7aa16c630
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -0,0 +1,196 @@
+/* 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 "NASController.h"
+
+#include <Main/Configs/NASConfig.h>
+#include <Main/Configs/SchedulerConfig.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <common/Topics.h>
+#include <events/EventBroker.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Main;
+using namespace Boardcore;
+using namespace Common;
+using namespace miosix;
+
+NASController::NASController()
+ : FSM{&NASController::state_init, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ Config::Scheduler::NAS_PRIORITY},
+ nas{Config::NAS::CONFIG}
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+ EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+ // TODO: Review this code
+ Eigen::Matrix<float, 13, 1> x = Eigen::Matrix<float, 13, 1>::Zero();
+ Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas.setX(x);
+ nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+}
+
+bool NASController::start()
+{
+ TaskScheduler &scheduler = getModule<BoardScheduler>()->getNasScheduler();
+
+ size_t result = scheduler.addTask([this]() { update(); }, Config::NAS::SAMPLE_RATE);
+
+ if (result == 0)
+ {
+ LOG_ERR(logger, "Failed to add NAS update task");
+ return false;
+ }
+
+ if (!FSM::start())
+ {
+ LOG_ERR(logger, "Failed to start NAS FSM");
+ return false;
+ }
+
+ return true;
+}
+
+NASControllerState NASController::getState()
+{
+ return state;
+}
+
+void NASController::update()
+{
+ // TODO:
+}
+
+void NASController::calibrate()
+{
+ // TODO:
+
+ EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+}
+
+
+void NASController::state_init(const Event& event) {
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(NASControllerState::INIT);
+ break;
+ }
+
+ case NAS_CALIBRATE:
+ {
+ transition(&NASController::state_calibrating);
+ break;
+ }
+ }
+}
+
+void NASController::state_calibrating(const Event& event) {
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(NASControllerState::CALIBRATING);
+ calibrate();
+ break;
+ }
+
+ case NAS_READY:
+ {
+ transition(&NASController::state_ready);
+ break;
+ }
+ }
+}
+
+void NASController::state_ready(const Event& event) {
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(NASControllerState::READY);
+ break;
+ }
+
+ case NAS_CALIBRATE:
+ {
+ transition(&NASController::state_calibrating);
+ break;
+ }
+
+ case NAS_FORCE_START:
+ case FLIGHT_ARMED:
+ {
+ transition(&NASController::state_active);
+ break;
+ }
+ }
+}
+
+void NASController::state_active(const Event& event) {
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(NASControllerState::ACTIVE);
+ break;
+ }
+ case FLIGHT_LANDING_DETECTED:
+ {
+ transition(&NASController::state_end);
+ break;
+ }
+ case NAS_FORCE_STOP:
+ case FLIGHT_DISARMED:
+ {
+ transition(&NASController::state_ready);
+ break;
+ }
+ }
+}
+
+void NASController::state_end(const Event& event) {
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ updateAndLogStatus(NASControllerState::END);
+ break;
+ }
+ }
+}
+
+void NASController::updateAndLogStatus(NASControllerState state)
+{
+ this->state = state;
+ NASControllerStatus data = {TimestampTimer::getTimestamp(), state};
+ sdLogger.log(data);
+}
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
new file mode 100644
index 000000000..0e699d3ad
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -0,0 +1,66 @@
+/* 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/BoardScheduler.h>
+#include <Main/StateMachines/NASController/NASControllerData.h>
+#include <algorithms/NAS/NAS.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class NASController : public Boardcore::FSM<NASController>,
+ public Boardcore::InjectableWithDeps<BoardScheduler>
+{
+public:
+ NASController();
+
+ [[nodiscard]] bool start() override;
+
+ NASControllerState getState();
+
+private:
+ void update();
+ void calibrate();
+
+ // FSM states
+ void state_init(const Boardcore::Event& event);
+ void state_calibrating(const Boardcore::Event& event);
+ void state_ready(const Boardcore::Event& event);
+ void state_active(const Boardcore::Event& event);
+ void state_end(const Boardcore::Event& event);
+
+ void updateAndLogStatus(NASControllerState state);
+
+ std::atomic<NASControllerState> state{NASControllerState::INIT};
+
+ Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ada");
+
+ Boardcore::NAS nas;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/NASController/NASControllerData.h b/src/boards/Main/StateMachines/NASController/NASControllerData.h
new file mode 100644
index 000000000..8d5136e77
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASControllerData.h
@@ -0,0 +1,54 @@
+/* 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 NASControllerState : uint8_t
+{
+ INIT = 0,
+ CALIBRATING,
+ READY,
+ ACTIVE,
+ END
+};
+
+struct NASControllerStatus
+{
+ uint64_t timestamp = 0;
+ NASControllerState state = NASControllerState::INIT;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << static_cast<int>(state) << "\n";
+ }
+};
+
+}
\ No newline at end of file
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index c56cad48f..d5fd219a0 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -28,6 +28,7 @@
#include <Main/Radio/Radio.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
#include <actuators/Servo/Servo.h>
#include <drivers/timer/PWM.h>
@@ -58,6 +59,7 @@ int main()
PinHandler *pinHandler = new PinHandler();
FlightModeManager *fmm = new FlightModeManager();
ADAController *ada = new ADAController();
+ NASController *nas = new NASController();
// Insert modules
bool initResult = manager.insert<Buses>(buses) &&
@@ -68,7 +70,9 @@ int main()
manager.insert<CanHandler>(canHandler) &&
manager.insert<PinHandler>(pinHandler) &&
manager.insert<FlightModeManager>(fmm) &&
- manager.insert<ADAController>(ada) && manager.inject();
+ manager.insert<ADAController>(ada) &&
+ manager.insert<NASController>(nas) &&
+ manager.inject();
manager.graphviz(std::cout);
@@ -144,6 +148,12 @@ int main()
std::cout << "Error failed to start ADAController" << std::endl;
}
+ if (!nas->start())
+ {
+ initResult = false;
+ std::cout << "Error failed to start NASController" << std::endl;
+ }
+
if (!fmm->start())
{
initResult = false;
--
GitLab