diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp index 015e946198a43714f51e01e1b440e15a308eccdb..9ca779b47735ee00419f59db80cfa47e2f8383f0 100644 --- a/src/RIGv2/Actuators/Actuators.cpp +++ b/src/RIGv2/Actuators/Actuators.cpp @@ -122,6 +122,8 @@ bool Actuators::ServoInfo::setOpeningTime(uint32_t time) } Actuators::Actuators() + : valveScheduler(STACK_DEFAULT_FOR_PTHREAD, + (miosix::Priority)(PRIORITY_MAX - 1)) { // Initialize servos infos[0].servo = std::make_unique<Servo>( @@ -276,14 +278,8 @@ bool Actuators::start() infos[8].servo->enable(); infos[9].servo->enable(); - miosix::Thread::create( - [](void* arg) -> void* - { - static_cast<Actuators*>(arg)->valveSchedulerTask(); - return nullptr; - }, - STACK_DEFAULT_FOR_PTHREAD, (miosix::Priority)(PRIORITY_MAX - 1), - static_cast<void*>(this)); + // Start the ValveScheduler thread + valveScheduler::start(); started = true; return true; @@ -610,43 +606,3 @@ void Actuators::updateNextActionTs() if (nextActionTs == std::numeric_limits<long long>::max()) nextActionTs = 0; } - -bool Actuators::terminateValveSchedulerTask() -{ - if (!isValveSchedulerAlive) - { - LOG_ERR(logger, "valve scheduler task is not currently running"); - return 0; - } - - isValveSchedulerAlive = 0; - return 1; -} - -void Actuators::valveSchedulerTask() -{ - isValveSchedulerAlive = 1; - - while (isValveSchedulerAlive) - { - std::unique_lock<std::mutex> lock(conditionVariableMutex); - - std::cv_status waitResult; - - if (nextActionTs == 0) - { - cv.wait(lock); - waitResult = std::cv_status::no_timeout; - } - else - { - waitResult = cv.wait_until( - lock, time_point<steady_clock>(nanoseconds(nextActionTs))); - } - - if (waitResult == std::cv_status::timeout) - updatePositions(); - - updateNextActionTs(); - } -} diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h index 564b5de8ad538ed2544e382b5c9f008fa06e2be1..ea6d151d91ecf5ceacfc3b9c1af0c7bb6e97034f 100644 --- a/src/RIGv2/Actuators/Actuators.h +++ b/src/RIGv2/Actuators/Actuators.h @@ -29,6 +29,7 @@ #include <common/MavlinkOrion.h> #include <miosix.h> #include <scheduler/TaskScheduler.h> +#include <scheduler/ValveScheduler.h> #include <condition_variable> #include <memory> @@ -37,7 +38,8 @@ namespace RIGv2 { class Actuators - : public Boardcore::InjectableWithDeps<BoardScheduler, CanHandler> + : public Boardcore::InjectableWithDeps<BoardScheduler, CanHandler>, + public Boardcore::valveScheduler { private: struct ServoInfo : public Boardcore::InjectableWithDeps<Registry> @@ -112,8 +114,6 @@ public: void inject(Boardcore::DependencyInjector& injector) override; - bool terminateValveSchedulerTask(); - private: ServoInfo* getServo(ServosList servo); @@ -121,11 +121,9 @@ private: void unsafeOpenChamber(); void unsafeCloseChamber(); - void updatePositions(); - - void updateNextActionTs(); + void updatePositions() override; - void valveSchedulerTask(); + void updateNextActionTs() override; Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators");