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