fixes to state machine

Author: caiodasilva2005

Core/Src/u_statemachine.c

@@ -54,7 +54,7 @@ void send_carstate_msg(void)
 		get_nero_state().home_mode,
 		get_nero_state().nero_index,
 		dti_get_mph(),
-		is_shutdown_closed(),
+			(),
 		pedals_getTorqueLimitPercentage(),
 		(cerberus_state.functional != F_REVERSE),
 		pedals_getRegenLimit(),
@@ -331,25 +331,5 @@ void statemachine_process(state_req_t new_state_req) {
 		else if(new_state_req.id == FUNCTIONAL) { transition_functional_state(new_state_req.state.functional); }
 	}
 
-	if (!is_ts_rising && is_shutdown_closed()) {
-		is_ts_rising = true;
-
-		/* Restart TS Rising timer. */
-		int status = timer_restart(&ts_rising_timer);
-		if(status != U_SUCCESS) {
-			PRINTLN_ERROR("Failed to restart TS Rising timer in `if (!ts_rising && !is_shutdown_closed())` (Status: %d).", status);
-			return;
-		}
-
-	} else if (!is_shutdown_closed()) {
-		/* Stop the TS Rising timer. */
-    	int status = timer_stop(&ts_rising_timer);
-    	if(status != U_SUCCESS) {
-        	PRINTLN_ERROR("Failed to stop TS Rising timer in `else if (!is_shutdown_closed())` (Status: %d).", status);
-        	return;
-    	}
-		is_ts_rising = false;
-		enter_drive_enabled = false;
-	}
 	send_carstate_msg();
 }

Core/Src/u_threads.c

@@ -268,18 +268,20 @@ static thread_t statemachine_thread = {
         .threshold  = 0,                      /* Preemption Threshold */
         .time_slice = TX_NO_TIME_SLICE,       /* Time Slice */
         .auto_start = TX_AUTO_START,          /* Auto Start */
-        .sleep      = 0,                      /* Sleep (in ticks) */
+        .sleep      = 200,                    /* Sleep (in ticks) */
         .function   = vStatemachine           /* Thread Function */
     };
 void vStatemachine(ULONG thread_input) {
 
     while(1) {
         state_req_t new_state_req;
-        while(queue_receive(&state_transition_queue, &new_state_req, TX_WAIT_FOREVER) == U_SUCCESS) {
-            statemachine_process(new_state_req);
-	    }
-
-        /* No sleep. Thread timing is controlled completely by the queue timeout. */
+        
+        // check if there is a transition in the queue, if not skip processing
+        UINT status = queue_receive(&state_transition_queue, &new_state_req, statemachine_thread.sleep);
+        if (status == U_SUCCESS) statemachine_process(new_state_req);
+        
+        // send state periodically whether receiving a transition or not
+        send_carstate_msg();
     }
 }
 
@@ -934,8 +936,6 @@ uint8_t threads_init(TX_BYTE_POOL *byte_pool) {
     CATCH_ERROR(create_thread(byte_pool, &efuses_thread), U_SUCCESS);              // Create eFuses thread.
     CATCH_ERROR(create_thread(byte_pool, &mux_thread), U_SUCCESS);               // Create Mux thread.
     CATCH_ERROR(create_thread(byte_pool, &peripherals_thread), U_SUCCESS);       // Create Peripherals thread.
-    // CATCH_ERROR(create_thread(byte_pool, &test_thread), U_SUCCESS);                // Create Test thread.
-    CATCH_ERROR(create_thread(byte_pool, &misc_telemetry_thread), U_SUCCESS);      // Create RTDS Telemetry thread.
 
     // add more threads here if need