diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 79c336a491c4f599d0ae3121f0e80e82db8d1890..1fdb17226ea6765da0b63743c7d3852d9c03a0c1 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 0000000000000000000000000000000000000000..46a3ee52ee50619a55808835f0251f9c1a5d96cd --- /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 99d398a6ebb1fd170148489592d67f0f9c897869..87d8480d591db796655c47f16fb2f392e0d6b1e0 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 fca5dd6dd28380d54671ee6b08f8d11d1c218613..81616e2dfb9218124cbdaba3a7e883dbe6c6ab12 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 bb803518585bec25b5170ae75fea94343b57021b..59f71c323241d3408b01fc50f5991f2fc75aca46 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 65c65ddcb10a4417bde2184eceb43bbd3815cb05..5999862d65ec2113b688bbc10ae8baa6919b6c4f 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 5d559a858b9b076049c92d186913b31d285494f6..6af46ed9884ede6b1f3765504189b76bca360ccb 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 d350b76fec9c4a8fc51278c7efb11d79a06a659e..4b6ae9b820d4a820cec8095b9d4e01193b197722 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 0000000000000000000000000000000000000000..7aa16c630aa80f0c48b48986ca24ab14159a3be9 --- /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 0000000000000000000000000000000000000000..0e699d3ad68e505706032de1ada58c961160b63e --- /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 0000000000000000000000000000000000000000..8d5136e77fd7dc42d11e4be498b1b98c889f9e35 --- /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 c56cad48f1ebf4de2fcc8d09b26fd90cf46aaa06..d5fd219a0de97b3a7e570fb5a3d2f7038a60864b 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;