add motor kv and tune to dronecan params

tridge · Sep 20, 2024 · 7e7a13c · 7e7a13c
1 parent 5be08e3
commit 7e7a13c
Showing 1 changed file with 41 additions and 0 deletions.
diff --git 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,31 @@ 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 +317,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;