diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp index 15a57f3744..c792101ad4 100644 --- a/src/drivers/cyphal/ParamManager.cpp +++ b/src/drivers/cyphal/ParamManager.cpp @@ -56,6 +56,15 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ } } + for (auto ¶m : _type_registers) { + if (strcmp(param_name, param.name) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } @@ -73,19 +82,33 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua } } + for (auto ¶m : _type_registers) { + if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name) { - if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder); + size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister); + if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + name.name.count = strlen(_uavcan_params[id].uavcan_name); + } else if (id < number_of_integer_registers + number_of_type_registers) { + id -= number_of_integer_registers; + strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name)); + name.name.count = strlen(_type_registers[id].name); + } else { return false; } - strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - - name.name.count = strlen(_uavcan_params[id].uavcan_name); - return true; } diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 3eab0818bc..056be73785 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -103,6 +103,10 @@ typedef struct { bool is_persistent {true}; } UavcanParamBinder; +typedef struct { + const char *name; + const char *value; +} CyphalTypeRegister; class UavcanParamManager { @@ -135,4 +139,10 @@ private: //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; + + CyphalTypeRegister _type_registers[3] { + {"uavcan.pub.udral.esc.0.type", "reg.udral.service.actuator.common.sp.Vector31"}, + {"uavcan.pub.udral.readiness.0.type", "reg.udral.service.common.Readiness.0.1"}, + {"uavcan.sub.zubax.feedback.0.type", "zubax.telega.CompactFeedback.0.1"}, + }; };