From c84d17a4d924ff3f11d274cd216204792ddda32c Mon Sep 17 00:00:00 2001
From: FReenen <git@finnvanreenen.nl>
Date: Thu, 5 Dec 2024 21:28:44 +0100
Subject: [PATCH] update servo trim settings

---
 rx_esp32/src/commands.c | 32 +++++++++++++++++++++++++++++++
 rx_esp32/src/main.c     |  2 +-
 rx_esp32/src/servos.c   | 42 ++++++++++-------------------------------
 rx_esp32/src/servos.h   |  3 ---
 4 files changed, 43 insertions(+), 36 deletions(-)

diff --git a/rx_esp32/src/commands.c b/rx_esp32/src/commands.c
index 22a1a16..9289f06 100644
--- a/rx_esp32/src/commands.c
+++ b/rx_esp32/src/commands.c
@@ -68,6 +68,37 @@ int cmd_contrl(char* line, void* cli)
 	return out;
 }
 
+int cmd_servotrim(char* line, void* cli)
+{
+	uint8_t ch;
+	int out = 0;
+	char msg[40];
+
+	char* arg = getNextArg(line, ':');
+	if (arg == NULL)
+	{
+		snprintf(&msg[0], 40, "no arguments given, expect at least one\n");
+		CLI_stringOut((CLI_t*)cli, &msg[0]);
+		out = -1;
+	}
+	else
+	{
+		readInt(arg, &ch);
+		arg = getNextArg(arg, ' ');
+		if (arg != NULL)
+		{
+			readInt(arg, &Servos[ch].pulse_min);
+			arg = getNextArg(arg, ':');
+			readInt(arg, &Servos[ch].pulse_mid);
+			arg = getNextArg(arg, ':');
+			readInt(arg, &Servos[ch].pulse_max);
+		}
+		snprintf(&msg[0], 40, "servo ch %d: %d:%d:%d\n", Servos[ch].pulse_min, Servos[ch].pulse_mid, Servos[ch].pulse_max);
+		CLI_stringOut((CLI_t*)cli, &msg[0]);
+	}
+	return out;
+}
+
 extern volatile bool running;
 int cmd_shutdown(char* line, void* cli)
 {
@@ -161,6 +192,7 @@ const CMD_t Commands[] = {
 #if TARGET == TARGET_RX
 	{ "d", &cmd_contrl },
 	{ "status", &cmd_status },
+	{ "servotrim", &cmd_servotrim},
 #elif TARGET == TARGET_TX
 	{ "led", &setLed },
 #endif
diff --git a/rx_esp32/src/main.c b/rx_esp32/src/main.c
index 2aba643..aab3f89 100644
--- a/rx_esp32/src/main.c
+++ b/rx_esp32/src/main.c
@@ -111,7 +111,7 @@ void app_main() {
 #if TARGET == TARGET_RX
 	CLI_t cli_ws_client;
 #elif TARGET == TARGET_TX
-   CLI_t cli_ws_client[3];
+	CLI_t cli_ws_client[3];
 #endif
 
 	LOG_D("main: main loop starting in state %d", MainState);
diff --git a/rx_esp32/src/servos.c b/rx_esp32/src/servos.c
index 1947eb3..8f84e5b 100644
--- a/rx_esp32/src/servos.c
+++ b/rx_esp32/src/servos.c
@@ -28,21 +28,15 @@ void servo_init(void)
 	Servos[0].pulse_min = SERVO_DUTY_MIN;
 	Servos[0].pulse_mid = SERVO_DUTY_MID;
 	Servos[0].pulse_max = SERVO_DUTY_MAX;
-	Servos[0].in_min = 0;
-	Servos[0].in_mid = 128;
-	Servos[0].in_max = 255;
 	Servos[0].mid_delay = 0;
 	Servos[0].mid_delay_timer = 0;
 	Servos[0].reversed = false;
 
 	Servos[1].pin = SERVOS_CH1_PIN;
 	Servos[1].ledc_ch = LEDC_CHANNEL_1;
-	Servos[1].pulse_min = SERVO_DUTY_MIN-35;
-	Servos[1].pulse_mid = SERVO_DUTY_MID-35;
-	Servos[1].pulse_max = SERVO_DUTY_MAX-35;
-	Servos[1].in_min = 0;
-	Servos[1].in_mid = 103;
-	Servos[1].in_max = 255;
+	Servos[1].pulse_min = SERVO_DUTY_MIN;
+	Servos[1].pulse_mid = SERVO_DUTY_MID;
+	Servos[1].pulse_max = SERVO_DUTY_MAX;
 	Servos[1].mid_delay = 5;
 	Servos[1].mid_delay_timer = 0;
 	Servos[1].reversed = true;
@@ -85,27 +79,11 @@ void servo_deinit(void)
 void servo_set(uint8_t ch, uint8_t pos)
 {
 	if (Servos_ch_swap)
-	{
-		if (ch == 0)
-		{
-			ch = 1;
-		}
-		else 
-		{
-			ch = 0;
-		}
+	{ 
+	 	ch = (ch == 0) ? 1 : 0;
 	}
 
-	double pos_double = ((double) pos) - Servos[ch].in_mid;
-
-	if (pos_double > 0.0)
-	{
-		pos_double = pos_double/(Servos[ch].in_max - Servos[ch].in_mid);
-	}
-	else 
-	{
-		pos_double = pos_double/(Servos[ch].in_mid - Servos[ch].in_min);
-	}
+	double pos_double = ((int16_t)pos - 128) / 128.0;
 
    if (Servos[ch].reversed)
 	{
@@ -113,7 +91,7 @@ void servo_set(uint8_t ch, uint8_t pos)
 	}
 
 
-	if (pos_double < 0)
+	if (pos_double < 0.0)
 	{
 		if (Servos[ch].mid_delay_timer > 0)
 		{
@@ -125,7 +103,7 @@ void servo_set(uint8_t ch, uint8_t pos)
 			Servos[ch].mid_delay_timer = -Servos[ch].mid_delay;
 		}
 	}
-	else if (pos_double > 0)
+	else if (pos_double > 0.0)
 	{
 		if (Servos[ch].mid_delay_timer < 0)
 		{
@@ -145,8 +123,8 @@ void servo_set(uint8_t ch, uint8_t pos)
 	}
 	else 
 	{
-      duty = (pos_double * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
-   }
+		duty = (pos_double * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
+	}
 
 	if (duty < SERVO_DUTY_MIN)
 	{
diff --git a/rx_esp32/src/servos.h b/rx_esp32/src/servos.h
index c680505..d8afd91 100644
--- a/rx_esp32/src/servos.h
+++ b/rx_esp32/src/servos.h
@@ -12,9 +12,6 @@ typedef struct {
 	uint16_t pulse_min;
 	uint16_t pulse_mid;
 	uint16_t pulse_max;
-	uint8_t in_min;
-	uint8_t in_mid;
-	uint8_t in_max;
 	uint8_t mid_delay;
 	int16_t mid_delay_timer;
 	bool reversed;