From 1f11d5f3fc957c7eb36529a23b516e735ce7967e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 15 Sep 2023 20:29:29 +0900 Subject: [PATCH] AP_DroneCAN: support string parameters --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 80 +++++++++++++++++++++++++-- libraries/AP_DroneCAN/AP_DroneCAN.h | 7 +++ 2 files changed, 81 insertions(+), 6 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index d55a0b1c5d..05a175e8cf 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1308,6 +1308,7 @@ void AP_DroneCAN::check_parameter_callback_timeout() param_request_sent_ms = 0; param_int_cb = nullptr; param_float_cb = nullptr; + param_string_cb = nullptr; } } @@ -1334,7 +1335,8 @@ bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, float // fail if waiting for any previous get/set request if (param_int_cb != nullptr || - param_float_cb != nullptr) { + param_float_cb != nullptr || + param_string_cb != nullptr) { return false; } param_getset_req.index = 0; @@ -1357,7 +1359,8 @@ bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32 // fail if waiting for any previous get/set request if (param_int_cb != nullptr || - param_float_cb != nullptr) { + param_float_cb != nullptr || + param_string_cb != nullptr) { return false; } param_getset_req.index = 0; @@ -1371,6 +1374,30 @@ bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32 return true; } +/* + set named string parameter on node +*/ +bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, const string &value, ParamGetSetStringCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + + // fail if waiting for any previous get/set request + if (param_int_cb != nullptr || + param_float_cb != nullptr || + param_string_cb != nullptr) { + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1); + memcpy(¶m_getset_req.value.string_value, (const void*)&value, sizeof(value)); + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + param_string_cb = cb; + param_request_sent = false; + param_request_sent_ms = AP_HAL::millis(); + param_request_node_id = node_id; + return true; +} + /* get named float parameter on node */ @@ -1380,7 +1407,8 @@ bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, Param // fail if waiting for any previous get/set request if (param_int_cb != nullptr || - param_float_cb != nullptr) { + param_float_cb != nullptr || + param_string_cb != nullptr) { return false; } param_getset_req.index = 0; @@ -1402,7 +1430,8 @@ bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, Param // fail if waiting for any previous get/set request if (param_int_cb != nullptr || - param_float_cb != nullptr) { + param_float_cb != nullptr || + param_string_cb != nullptr) { return false; } param_getset_req.index = 0; @@ -1415,11 +1444,35 @@ bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, Param return true; } +/* + get named string parameter on node +*/ +bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetStringCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + + // fail if waiting for any previous get/set request + if (param_int_cb != nullptr || + param_float_cb != nullptr || + param_string_cb != nullptr) { + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)); + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + param_string_cb = cb; + param_request_sent = false; + param_request_sent_ms = AP_HAL::millis(); + param_request_node_id = node_id; + return true; +} + void AP_DroneCAN::handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp) { WITH_SEMAPHORE(_param_sem); if (!param_int_cb && - !param_float_cb) { + !param_float_cb && + !param_string_cb) { return; } if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) && param_int_cb) { @@ -1448,13 +1501,28 @@ void AP_DroneCAN::handle_param_get_set_response(const CanardRxTransfer& transfer param_request_node_id = transfer.source_node_id; return; } + } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) && param_string_cb) { + string val; + memcpy(&val, &rsp.value.string_value, sizeof(val)); + if ((*param_string_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) { + // we want the parameter to be set with val + param_getset_req.index = 0; + memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len); + memcpy(¶m_getset_req.value.string_value, &val, sizeof(val)); + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + param_request_sent = false; + param_request_sent_ms = AP_HAL::millis(); + param_request_node_id = transfer.source_node_id; + return; + } } + param_request_sent_ms = 0; param_int_cb = nullptr; param_float_cb = nullptr; + param_string_cb = nullptr; } - void AP_DroneCAN::send_parameter_save_request() { WITH_SEMAPHORE(_param_save_sem); diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index ed1d79e70b..81abfbdda4 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -78,8 +78,12 @@ public: uint8_t get_driver_index() const { return _driver_index; } + // define string with length structure + struct string { uint8_t len; uint8_t data[128]; }; + FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + FUNCTOR_TYPEDEF(ParamGetSetStringCb, bool, AP_DroneCAN*, const uint8_t, const char*, string &); FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_DroneCAN*, const uint8_t, bool); void send_node_status(); @@ -104,8 +108,10 @@ public: // failures occur when waiting on node to respond to previous get or set request bool set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb); bool set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb); + bool set_parameter_on_node(uint8_t node_id, const char *name, const string &value, ParamGetSetStringCb *cb); bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb); bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb); + bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetStringCb *cb); // Save parameters bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb); @@ -177,6 +183,7 @@ private: // get parameter on a node ParamGetSetIntCb *param_int_cb; // latest get param request callback function (for integers) ParamGetSetFloatCb *param_float_cb; // latest get param request callback function (for floats) + ParamGetSetStringCb *param_string_cb; // latest get param request callback function (for strings) bool param_request_sent = true; // true after a param request has been sent, false when queued to be sent uint32_t param_request_sent_ms; // system time that get param request was sent HAL_Semaphore _param_sem; // semaphore protecting this block of variables