From 5f388d53a6b0742ae758d2841e36335196bb2ad3 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Sat, 27 Jan 2024 10:33:49 +0100
Subject: [PATCH] reset RW cmds to 0 if they are not used anymore

---
 mission/controller/AcsController.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 8e7ae8d4..43e2ccb6 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -885,6 +885,24 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
 }
 
 void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
+  if (mode == acs::AcsMode::SAFE) {
+    {
+      PoolReadGuard pg(&rw1SpeedSet);
+      rw1SpeedSet.setRwSpeed(0, 10);
+    }
+    {
+      PoolReadGuard pg(&rw2SpeedSet);
+      rw2SpeedSet.setRwSpeed(0, 10);
+    }
+    {
+      PoolReadGuard pg(&rw3SpeedSet);
+      rw3SpeedSet.setRwSpeed(0, 10);
+    }
+    {
+      PoolReadGuard pg(&rw4SpeedSet);
+      rw4SpeedSet.setRwSpeed(0, 10);
+    }
+  }
   return ExtendedControllerBase::modeChanged(mode, submode);
 }