diff --git a/src/boards/HeliTest/Events.h b/src/boards/HeliTest/Events.h index 1f233c0ec8c45a647ce562709ed16a0805e87056..c3cfbde6c18e7d2ca4ef7bbd9e44c27126560a8a 100644 --- a/src/boards/HeliTest/Events.h +++ b/src/boards/HeliTest/Events.h @@ -29,5 +29,5 @@ enum HeliEvents : uint8_t EV_HELIPIN_DISCONNECTED = EV_FIRST_UNSHARED_SIGNAL, EV_PARACHUTEPIN_DISCONNECTED, EV_START_CUTTING, - EV_STOP_CUTTING + EV_TIMEOUT_STOP_CUTTING }; \ No newline at end of file diff --git a/src/boards/HeliTest/FlightModeManager/HeliFMM.cpp b/src/boards/HeliTest/FlightModeManager/HeliFMM.cpp index bd21ca2ca8dc0d4467de2eeaf967a0ee4cfb7fc2..d07ea54019615b0d24817d7ca05ee5d6295d0278 100644 --- a/src/boards/HeliTest/FlightModeManager/HeliFMM.cpp +++ b/src/boards/HeliTest/FlightModeManager/HeliFMM.cpp @@ -37,7 +37,8 @@ using miosix::Lock; namespace RogallinaBoard { -HeliFMM::HeliFMM() : FSM(&HeliFMM::bootedState) +// Max priority (4) for the Flight Mode Manager +HeliFMM::HeliFMM(Cutter& cutter) : FSM(&HeliFMM::bootedState), cutter(cutter) { sEventBroker->subscribe(this, TOPIC_USER_INPUT); sEventBroker->subscribe(this, TOPIC_STATUS); @@ -46,9 +47,7 @@ HeliFMM::HeliFMM() : FSM(&HeliFMM::bootedState) memset(&status, 0, sizeof(HeliStatus)); } -HeliFMM::~HeliFMM() { - sEventBroker->unsubscribe(this); -} +HeliFMM::~HeliFMM() { sEventBroker->unsubscribe(this); } HeliStatus HeliFMM::getStatus() { return status; } @@ -65,14 +64,18 @@ void HeliFMM::bootedState(const Event& ev) } case EV_EXIT: break; +#ifndef IGNORE_LOGGER_NOT_WORKING case EV_LOGGER_ERROR: transition(&HeliFMM::failedState); status.failReason = FMMFAIL_LOGGER; break; +#endif +#ifndef IGNORE_SENSORS_NOT_WORKING case EV_SENSOR_INIT_ERROR: transition(&HeliFMM::failedState); status.failReason = FMMFAIL_SENSOR; break; +#endif case EV_BUTTON_LONG_PRESS: transition(&HeliFMM::readyState); break; @@ -90,7 +93,7 @@ void HeliFMM::failedState(const Event& ev) { case EV_ENTRY: status.state = HELISTATE_FAILED; - + logStatus(); TRACE("[FMM] Entering failed state.\n"); break; case EV_EXIT: @@ -138,13 +141,14 @@ void HeliFMM::freeFallingState(const Event& ev) switch (ev.sig) { case EV_ENTRY: - status.state = HELISTATE_FREEFALL; + status.state = HELISTATE_FREEFALL; + status.time_free_fall = miosix::getTick(); logStatus(); - TRACE("[FMM] Entering freefall state.\n"); delayed_ev_id = sEventBroker->postDelayed( {EV_START_CUTTING}, TOPIC_FSM, FREEFALL_TIME_MS); + TRACE("[FMM] Entering freefall state.\n"); break; case EV_START_CUTTING: transition(&HeliFMM::cuttingState); @@ -159,19 +163,24 @@ void HeliFMM::cuttingState(const Event& ev) switch (ev.sig) { case EV_ENTRY: - sEventBroker->post({EV_START_CUTTING}, TOPIC_COORDINATION); - - status.state = HELISTATE_CUTTING; + delayed_ev_id = sEventBroker->postDelayed( + {EV_TIMEOUT_STOP_CUTTING}, TOPIC_FSM, TIMEOUT_STOP_CUTTING); + cutter.startCut(); + status.time_start_cut = miosix::getTick(); + status.state = HELISTATE_CUTTING; logStatus(); TRACE("[FMM] Entering cutting state.\n"); break; + case EV_TIMEOUT_STOP_CUTTING: case EV_PARACHUTEPIN_DISCONNECTED: transition(&HeliFMM::descendingState); break; case EV_EXIT: - sEventBroker->post({EV_STOP_CUTTING}, TOPIC_COORDINATION); + cutter.stopCut(); + + sEventBroker->removeDelayed(delayed_ev_id); break; } } @@ -181,7 +190,8 @@ void HeliFMM::descendingState(const Event& ev) switch (ev.sig) { case EV_ENTRY: - status.state = HELISTATE_DESCENDING; + status.time_end_cut = miosix::getTick(); + status.state = HELISTATE_DESCENDING; logStatus(); TRACE("[FMM] Entering descending state.\n"); @@ -237,4 +247,4 @@ void HeliFMM::handleEvent(const Event& ev) miosix::MemoryProfiling::getAbsoluteFreeStack()}); } -} \ No newline at end of file +} // namespace RogallinaBoard \ No newline at end of file diff --git a/src/boards/HeliTest/FlightModeManager/HeliFMM.h b/src/boards/HeliTest/FlightModeManager/HeliFMM.h index 917a3c9f7c79b2eb7e2c25ee5efe02ad3a792168..87d9ffc4ff4dd97fc1e400a545fe27802b55bd71 100644 --- a/src/boards/HeliTest/FlightModeManager/HeliFMM.h +++ b/src/boards/HeliTest/FlightModeManager/HeliFMM.h @@ -29,6 +29,7 @@ #include "Singleton.h" #include "events/FSM.h" #include "logger/Logger.h" +#include "HeliTest/ThermalCutter/Cutter.h" using miosix::FastMutex; using std::array; @@ -36,20 +37,16 @@ using std::array; namespace RogallinaBoard { -class HeliFMM : public FSM<HeliFMM>, public Singleton<HeliFMM> +class HeliFMM : public FSM<HeliFMM> { - friend class Singleton<HeliFMM>; - public: + HeliFMM(Cutter& cutter); + ~HeliFMM(); + HeliStatus getStatus(); -protected: void handleEvent(const Event& ev) override; -private: - HeliFMM(); - ~HeliFMM(); - void bootedState(const Event& ev); void failedState(const Event& ev); void calibrationState(const Event& ev); @@ -70,6 +67,8 @@ private: HeliStatus status; Logger& log = Logger::instance(); + + Cutter& cutter; }; #define sHeliFMM HeliFMM::getInstance() diff --git a/src/boards/HeliTest/config.h b/src/boards/HeliTest/config.h index cd4f5afbb1f2710e904dde0056f6d38cd72e5564..a059f69a90912e3ad6b829033561979d73ca8979 100644 --- a/src/boards/HeliTest/config.h +++ b/src/boards/HeliTest/config.h @@ -43,7 +43,7 @@ static const PWM::Timer CUTTER_TIM{ TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; using CUTTER_ENA = interfaces::actuators::hbridgel::inh; -using CUTTER_IN = interfaces::actuators::servor::ctrl; +using CUTTER_IN = interfaces::actuators::servol::ctrl; using CUTTER_CSENSE = miosix::Gpio<GPIOC_BASE, 4>; /* @@ -52,6 +52,7 @@ using CUTTER_CSENSE = miosix::Gpio<GPIOC_BASE, 4>; // Time from pin detachment to cutter activation in ms static const unsigned int FREEFALL_TIME_MS = 500; +static const unsigned int TIMEOUT_STOP_CUTTING = 1000; static const unsigned int BRIDGE_DISCHARGE_TIME_MS = 30; diff --git a/src/entrypoints/heli-entry.cpp b/src/entrypoints/heli-entry.cpp index 64cf36c173e3996f3f8b391fec82a3b5d6ce0c1c..6ae5378929ec29ad923bc7bd5c156eb2e8d83155 100644 --- a/src/entrypoints/heli-entry.cpp +++ b/src/entrypoints/heli-entry.cpp @@ -34,6 +34,7 @@ #include "HeliTest/FlightModeManager/HeliFMM.h" #include "HeliTest/PinObserverWrapper.h" #include "HeliTest/ScreenManager.h" +#include "HeliTest/ThermalCutter/Cutter.h" #include "HeliTest/config.h" #include "PinStatus.h" #include "SensorManager/SensorManager.h" @@ -55,6 +56,7 @@ TaskScheduler* scheduler; SensorManager* sensorManager; ScreenManager* screen; +Cutter* cutter; HeliFMM* fmm; PinObserverWrapper* pinObserver; Logger& logger = Logger::instance(); @@ -65,17 +67,18 @@ void onButtonPressCallback(ButtonHandler_t::ButtonPress type); void initPins() { - { FastInterruptDisableLock d; DETACHMENT_PIN::mode(DETACHMENT_PIN_MODE); CUTTER_ENA::mode(Mode::OUTPUT); + CUTTER_IN::mode(Mode::ALTERNATE); CUTTER_IN::alternateFunction(2); + CUTTER_CSENSE::mode(Mode::INPUT_ANALOG); } - + interfaces::buttons::user::mode(Mode::INPUT); red_led::mode(Mode::OUTPUT); green_led::mode(Mode::OUTPUT); @@ -88,14 +91,14 @@ void initComponents() { EventBroker::getInstance(); - btnHandler = new ButtonHandler_t(1, &onButtonPressCallback); - scheduler = new TaskScheduler(); - fmm = HeliFMM::getInstance(); - + btnHandler = new ButtonHandler_t(1, &onButtonPressCallback); + scheduler = new TaskScheduler(); + cutter = new Cutter(); + fmm = new HeliFMM(*cutter); sensorManager = new SensorManager(scheduler); - screen = new ScreenManager(sensorManager); + pinObserver = new PinObserverWrapper(); - pinObserver = new PinObserverWrapper(); + screen = new ScreenManager(sensorManager, fmm, pinObserver); } int main() @@ -105,8 +108,8 @@ int main() initComponents(); // Start the active objects - scheduler->start(); - sEventBroker->start(); + scheduler->start(4096, 2); + sEventBroker->start(2048, 3); #ifdef SD_LOGGING try @@ -120,11 +123,11 @@ int main() } #endif - btnHandler->start(); - fmm->start(); - sensorManager->start(); + btnHandler->start(2048, 1); + fmm->start(4096, 3); + sensorManager->start(4096, 1); - pinObserver->start(); + pinObserver->start(2048, 3); sEventBroker->postDelayed({EV_START_SAMPLING}, TOPIC_COORDINATION, 100);