From b1c79db763db890beefc9eeb48d95dc3a1415a27 Mon Sep 17 00:00:00 2001 From: Davide Mor <davide.mor@skywarder.eu> Date: Mon, 26 Feb 2024 10:37:34 +0100 Subject: [PATCH] [RIGv2] Made Actuators actually stoppable --- src/boards/RIGv2/Actuators/Actuators.cpp | 23 +++++++++++++++-------- src/boards/RIGv2/Actuators/Actuators.h | 1 + 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp index 6e5d69fc8..73e81f65a 100644 --- a/src/boards/RIGv2/Actuators/Actuators.cpp +++ b/src/boards/RIGv2/Actuators/Actuators.cpp @@ -119,13 +119,21 @@ bool Actuators::start() infos[8].servo->enable(); infos[9].servo->enable(); - size_t result = - scheduler.addTask([this]() { updatePositionsTask(); }, - Config::Servos::SERVO_TIMINGS_CHECK_PERIOD); - if (result == 0) + if (updatePositionTaskId == 0) { - LOG_ERR(logger, "Failed to add updatePositionsTask"); - return false; + updatePositionTaskId = + scheduler.addTask([this]() { updatePositionsTask(); }, + Config::Servos::SERVO_TIMINGS_CHECK_PERIOD); + + if (updatePositionTaskId == 0) + { + LOG_ERR(logger, "Failed to add updatePositionsTask"); + return false; + } + } + else + { + scheduler.enableTask(updatePositionTaskId); } return true; @@ -144,8 +152,7 @@ void Actuators::stop() infos[8].servo->disable(); infos[9].servo->disable(); - // We just can only hope the scheduler is also disabled, otherwise bad - // things will happen + scheduler.disableTask(updatePositionTaskId); } bool Actuators::wiggleServo(ServosList servo) diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h index 008ffcf97..f8fe6edc2 100644 --- a/src/boards/RIGv2/Actuators/Actuators.h +++ b/src/boards/RIGv2/Actuators/Actuators.h @@ -84,6 +84,7 @@ private: Boardcore::TaskScheduler &scheduler; + size_t updatePositionTaskId = 0; miosix::FastMutex infosMutex; ServoInfo infos[10] = {}; }; -- GitLab