From 4f5cd03b9e84653baf317df2a1e433c6f5d746d1 Mon Sep 17 00:00:00 2001
From: Huibean <huibean.luo@gmail.com>
Date: Fri, 20 Sep 2024 18:26:00 +0800
Subject: [PATCH] add motor kv and tune to dronecan params

---
 Src/DroneCAN/DroneCAN.c | 42 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/Src/DroneCAN/DroneCAN.c b/Src/DroneCAN/DroneCAN.c
index 1fa740d2..cb9abcd3 100644
--- a/Src/DroneCAN/DroneCAN.c
+++ b/Src/DroneCAN/DroneCAN.c
@@ -32,6 +32,10 @@
 #define CANARD_POOL_SIZE 4096
 #endif
 
+#define EEPROM_MOTOR_KV_INDEX 26
+#define EEPROM_TUNE_INDEX 48
+#define EEPROM_TUNE_MAX_LEN 128
+
 static CanardInstance canard;
 static uint8_t canard_memory_pool[CANARD_POOL_SIZE];
 
@@ -69,6 +73,8 @@ static struct
 enum VarType {
     T_BOOL = 0,
     T_UINT8,
+    T_UINT16,
+    T_STRING,
 };
 
 static void can_printf(const char *fmt, ...);
@@ -85,6 +91,7 @@ extern char dir_reversed;
 extern char bi_direction;
 extern char advance_level;
 extern char motor_poles;
+extern uint16_t motor_kv;
 extern char VARIABLE_PWM;
 extern char use_sin_start;
 extern char comp_pwm;
@@ -109,6 +116,7 @@ static struct parameter {
     { "CAN_NODE",               T_UINT8, 0, 127, &settings.can_node, 176},
     { "ESC_INDEX",              T_UINT8, 0, 32,  &settings.esc_index, 177},
     { "DIR_REVERSED",           T_BOOL,  0, 1,   &dir_reversed, 0 },
+    { "MOTOR_KV",               T_UINT16,0, 10220, &motor_kv, EEPROM_MOTOR_KV_INDEX},
     { "BI_DIRECTIONAL",         T_BOOL,  0, 1,   &bi_direction, 0 },
     { "MOTOR_POLES",            T_UINT8, 0, 64,  &motor_poles, 27 },
     { "REQUIRE_ARMING",         T_BOOL,  0, 1,   &settings.require_arming, 178 },
@@ -118,6 +126,7 @@ static struct parameter {
     { "COMP_PWM",               T_BOOL,  0, 1,   &comp_pwm, 0},
     { "STUCK_ROTOR_PROTECTION", T_BOOL,  0, 1,   &stuck_rotor_protection, 0},
     { "ADVANCE_LEVEL",          T_UINT8, 0, 4,   &advance_level, 0},
+    { "TUNE",                   T_STRING,0, 4,   NULL, EEPROM_TUNE_INDEX},
 };
 
 /*
@@ -263,12 +272,32 @@ static void handle_param_GetSet(CanardInstance* ins, CanardRxTransfer* transfer)
 		eepromBuffer[p->eeprom_index] = *(uint8_t *)p->ptr;
 	    }
             break;
+	case T_UINT16:
+	    *(uint16_t *)p->ptr = req.value.integer_value;
+        if (p->eeprom_index == EEPROM_MOTOR_KV_INDEX) {
+            eepromBuffer[EEPROM_MOTOR_KV_INDEX] = (uint8_t)((*(uint16_t *)p->ptr - 20) / 40);
+        }
+        break;
 	case T_BOOL:
 	    *(uint8_t *)p->ptr = req.value.boolean_value?1:0;
 	    if (p->eeprom_index != 0) {
 		eepromBuffer[p->eeprom_index] = *(uint8_t *)p->ptr;
 	    }
             break;
+	case T_STRING:
+		if (req.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) {
+			if (p->eeprom_index == EEPROM_TUNE_INDEX) {
+				for (size_t i = 0; i < EEPROM_TUNE_MAX_LEN; i++)
+				{
+					if (i < req.value.string_value.len) {
+						eepromBuffer[EEPROM_TUNE_INDEX + i] = req.value.string_value.data[i];
+					}  else {
+						eepromBuffer[EEPROM_TUNE_INDEX + i] = 0xFF;
+					}
+				}
+			}
+		}
+		break;
 	default:
             return;
 	}
@@ -289,6 +318,19 @@ static void handle_param_GetSet(CanardInstance* ins, CanardRxTransfer* transfer)
 	    pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
 	    pkt.value.integer_value = *(uint8_t *)p->ptr;
             break;
+	case T_UINT16:
+	    pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+	    pkt.value.integer_value = *(uint16_t *)p->ptr;
+        break;
+	case T_STRING:
+		pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
+		if (p->eeprom_index == EEPROM_TUNE_INDEX) {
+			pkt.value.string_value.len = EEPROM_TUNE_MAX_LEN;
+			for (size_t i=0; i < EEPROM_TUNE_MAX_LEN; i++) {
+				pkt.value.string_value.data[i] = eepromBuffer[EEPROM_TUNE_INDEX + i];
+			}
+		}
+		break;
 	case T_BOOL:
 	    pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE;
 	    pkt.value.boolean_value = (*(uint8_t *)p->ptr)?true:false;