aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DroneCAN: support string parameters

Browse Source
This commit is contained in:
Randy Mackay 2023-09-15 20:29:29 +09:00 committed by Andrew Tridgell
parent 2835deb0de
commit 1f11d5f3fc
2 changed files with 81 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -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(&param_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(&param_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);

7
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -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