From 5f54681bbf9c7303b0c843d95d9db6a35cb248cc Mon Sep 17 00:00:00 2001
From: jonny_jr9 <jonny@wwad.de>
Date: Wed, 13 Sep 2023 12:15:35 +0200
Subject: [PATCH] Sabertooth: Add uart-mutex; Add both-off serial command

- add mutex around mutex send instruction
- add minimum delay between sent messages
- detect situation where both motors are off and send special both motor
  stop command. This might fix issue "right motor stay always on"
---
 common/motordrivers.cpp | 21 +++++++++++++++++++--
 common/motordrivers.hpp | 18 ++++++++++++++++--
 2 files changed, 35 insertions(+), 4 deletions(-)

diff --git a/common/motordrivers.cpp b/common/motordrivers.cpp
index 4fa581f..2cae1d8 100644
--- a/common/motordrivers.cpp
+++ b/common/motordrivers.cpp
@@ -206,6 +206,8 @@ void sabertooth2x60a::init(){
 	ESP_ERROR_CHECK(uart_set_pin(config.uart_num, config.gpio_TX, GPIO_NUM_NC, 0, 0));
 	ESP_LOGI(TAG, "init...");
 	ESP_ERROR_CHECK(uart_driver_install(config.uart_num, 1024, 1024, 10, NULL, 0)); //todo ring buffer less / 0
+																					//
+	uart_mutex = xSemaphoreCreateMutex();
 	uart_isInitialized = true;
 }
 
@@ -235,14 +237,20 @@ float motorCommandToSignedDuty(motorCommand_t cmd){
 
 //-------- send byte --------
 //send byte via uart to sabertooth driver
+#define UART_MUTEX_TIMEOUT_MS 5000
 void sabertooth2x60a::sendByte(char data){
 	//TODO mutex?
 	if (!uart_isInitialized){
 		ESP_LOGE(TAG, "uart not initialized, not sending command %d...", data);
 		return;
 	}
-	uart_write_bytes(config.uart_num, &data, 1);
-	ESP_LOGD(TAG, "sent data=%d to sabertooth driver via uart", data);
+	if (xSemaphoreTake(uart_mutex,  pdMS_TO_TICKS(UART_MUTEX_TIMEOUT_MS)) == pdTRUE) {
+		uart_write_bytes(config.uart_num, &data, 1);
+		ESP_LOGD(TAG, "sent data=%d to sabertooth driver via uart", data);
+		//note 1byte takes 833us at 9600 baud
+		ets_delay_us(400); //wait for min time between each message
+		xSemaphoreGive(uart_mutex);
+	} else ESP_LOGE(TAG, "timeout waiting for uart mutex");
 }
 
 
@@ -270,6 +278,11 @@ void sabertooth2x60a::setLeft(float dutyPerSigned){
 		//scale positive values between 0 and 100 to control motor 1
 		data = static_cast<int>(64 + (dutyPerSigned / 100.0) * 63);
 	}
+
+	//check if both motors are off -> special off command
+	lastDataLeft = data;
+	if (data == 64 && lastDataRight == 192) data = 0;
+
 	ESP_LOGI(TAG, "set left motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
 	sendByte(data);
 }
@@ -299,6 +312,10 @@ void sabertooth2x60a::setRight(float dutyPerSigned) {
 		// Scale positive values between 0 and 100 to control motor 2
 		data = static_cast<uint8_t>(192 + (dutyPerSigned / 100.0) * 63);
 	}
+	
+	//check if both motors are off -> special off command
+	lastDataRight = data;
+	if (data == 192 && lastDataLeft == 64) data = 0;
 
 	ESP_LOGI(TAG, "set right motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
 	sendByte(data);
diff --git a/common/motordrivers.hpp b/common/motordrivers.hpp
index 894013d..fa92a0a 100644
--- a/common/motordrivers.hpp
+++ b/common/motordrivers.hpp
@@ -10,15 +10,25 @@ extern "C"
 #include "driver/ledc.h"
 #include "esp_err.h"
 #include "driver/uart.h"
+#include "freertos/semphr.h"
 }
 
 #include <cmath>
+#include "types.hpp"
+
+
+
+//-------------------------------
+//----- CommandToSignedDuty -----
+//-------------------------------
+//convert motor command (FWD/REV/INDLE, duty) to value from -100 to 100
+float motorCommandToSignedDuty(motorCommand_t cmd);
+
 
 
 //====================================
 //===== single100a motor driver ======
 //====================================
-
 //--------------------------------------------
 //---- struct, enum, variable declarations ---
 //--------------------------------------------
@@ -40,7 +50,6 @@ typedef struct single100a_config_t {
 } single100a_config_t;
 
 
-
 //--------------------------------
 //------- single100a class -------
 //--------------------------------
@@ -112,6 +121,11 @@ class sabertooth2x60a {
 		uint32_t dutyMax;
 		motorstate_t state = motorstate_t::IDLE;
 		bool uart_isInitialized = false;
+		SemaphoreHandle_t uart_mutex;
+		uint8_t lastDataLeft = 0;
+		uint8_t lastDataRight = 0;
+
+		
 };