From b27e69042c4c75588325da482c832be2e36d78f8 Mon Sep 17 00:00:00 2001
From: bjackson312006 <blakejackson312006@gmail.com>
Date: Thu, 23 Apr 2026 20:46:56 -0400
Subject: [PATCH] rtds adjustements

---
 Core/Inc/u_rtds.h         |  2 +-
 Core/Src/u_rtds.c         | 75 ++++++++++++++++++++-------------------
 Core/Src/u_statemachine.c |  6 +++-
 3 files changed, 44 insertions(+), 39 deletions(-)

diff --git a/Core/Inc/u_rtds.h b/Core/Inc/u_rtds.h
index 722d8b8..f623883 100644
--- a/Core/Inc/u_rtds.h
+++ b/Core/Inc/u_rtds.h
@@ -4,7 +4,7 @@
 #include <stdbool.h>
 
 /* Config: */
-#define RTDS_DURATION 5000 /* RTDS duration (in ticks). */
+#define RTDS_DURATION 1500 /* RTDS duration (in ticks). */
 
 /* API */
 int rtds_init(void);                // Initializes the RTDS timer.
diff --git a/Core/Src/u_rtds.c b/Core/Src/u_rtds.c
index b4f1f9a..5290ec2 100644
--- a/Core/Src/u_rtds.c
+++ b/Core/Src/u_rtds.c
@@ -95,26 +95,6 @@ int rtds_init(void) {
         return U_SUCCESS;
 }
 
-/* Sounds the RTDS (Ready-to-drive sound). */
-int rtds_soundRTDS(void) {
-
-    /* Stop the reverse sound if active. */
-    rtds_stopReverseSound();
-
-    /* Trigger the RTDS sound. */
-    _set_rtds_pin();
-
-    /* Restart RTDS timer. */
-    int status = timer_restart(&rtds_timer);
-    if(status != U_SUCCESS) {
-        PRINTLN_ERROR("Failed to restart RTDS timer (Status: %d).", status);
-        queue_send(&faults, &(fault_t){RTDS_FAULT}, TX_NO_WAIT);
-        return U_ERROR;
-    }
-
-    return U_SUCCESS;
-}
-
 /* Cancels the RTDS sound. The RTDS sound already stops automatically once it's over, but this function lets you cancel it prematurely. */
 int rtds_cancelRTDS(void) {
     /* Stop the RTDS sound. */
@@ -148,23 +128,6 @@ int rtds_startReverseSound(void) {
     return U_SUCCESS;
 }
 
-/* Stops the reverse sound. */
-int rtds_stopReverseSound(void) {
-    /* Turn off the RTDS sound */
-    _clear_rtds_pin();
-
-    /* Deactivate the reverse sound timer */
-    int status = timer_stop(&reverse_sound_timer);
-    if(status != U_SUCCESS) {
-        PRINTLN_ERROR("Failed to deactivate reverse sound timer (Status: %d).", status);
-        queue_send(&faults, &(fault_t){RTDS_FAULT}, TX_NO_WAIT);
-        return U_ERROR;
-    }
-
-    PRINTLN_INFO("Stopped reverse sound.");
-    return U_SUCCESS;
-}
-
 /* Reads the status of the RTDS pin (true = RTDS pin is active, false = RTDS pin is not active). */
 /* Useful for debugging. */
 bool rtds_readRTDS(void) {
@@ -212,3 +175,41 @@ int rtds_isInSounding(bool* state) {
 
     return U_SUCCESS;
 }
+
+/* Stops the reverse sound. */
+int rtds_stopReverseSound(void) {
+
+    /* Turn off the RTDS sound */
+    _clear_rtds_pin();
+
+    /* Deactivate the reverse sound timer */
+    int status = timer_stop(&reverse_sound_timer);
+    if(status != U_SUCCESS) {
+        PRINTLN_ERROR("Failed to deactivate reverse sound timer (Status: %d).", status);
+        queue_send(&faults, &(fault_t){RTDS_FAULT}, TX_NO_WAIT);
+        return U_ERROR;
+    }
+
+    PRINTLN_INFO("Stopped reverse sound.");
+    return U_SUCCESS;
+}
+
+/* Sounds the RTDS (Ready-to-drive sound). */
+int rtds_soundRTDS(void) {
+
+    /* Stop the reverse sound if active. */
+    rtds_stopReverseSound();
+
+    /* Trigger the RTDS sound. */
+    _set_rtds_pin();
+
+    /* Restart RTDS timer. */
+    int status = timer_restart(&rtds_timer);
+    if(status != U_SUCCESS) {
+        PRINTLN_ERROR("Failed to restart RTDS timer (Status: %d).", status);
+        queue_send(&faults, &(fault_t){RTDS_FAULT}, TX_NO_WAIT);
+        return U_ERROR;
+    }
+
+    return U_SUCCESS;
+}
diff --git a/Core/Src/u_statemachine.c b/Core/Src/u_statemachine.c
index cb9d169..60d4b57 100644
--- a/Core/Src/u_statemachine.c
+++ b/Core/Src/u_statemachine.c
@@ -111,7 +111,11 @@ static int transition_functional_state(func_state_t new_state)
 
 	/* Make sure wheels are not spinning before changing modes */
 	bool brake_state;
-	rtds_stopReverseSound();
+	
+	/* If we're actively in the reverse state, stop the reverse sound before doing any state changes. */
+	if(cerberus_state.functional == F_REVERSE) {
+		rtds_stopReverseSound();
+	}
 
 	/* Catching state transitions */
 	switch (new_state) {