RTDS Adjustements

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.

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;
+}

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) {