Skip to content

Commit

Permalink
AP_UAVCAN: support string parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Nov 6, 2023
1 parent c9de405 commit 3716e34
Show file tree
Hide file tree
Showing 2 changed files with 86 additions and 7 deletions.
86 changes: 79 additions & 7 deletions libraries/AP_UAVCAN/AP_UAVCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1647,6 +1647,7 @@ void AP_UAVCAN::check_parameter_callback_timeout()
param_request_sent_ms = 0;
param_int_cb = nullptr;
param_float_cb = nullptr;
param_string_cb = nullptr;
}
}

Expand All @@ -1673,7 +1674,8 @@ bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, float v

// 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[_driver_index].index = 0;
Expand All @@ -1695,7 +1697,8 @@ bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t

// 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[_driver_index].index = 0;
Expand All @@ -1709,15 +1712,42 @@ bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t
}

/*
get named integer parameter on node
set named string parameter on node
*/
bool AP_UAVCAN::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[_driver_index].index = 0;
param_getset_req[_driver_index].name = name;
//memcpy(&param_getset_req[_driver_index].value.c_str(), &value, sizeof(value));
//memcpy(&param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::string_value>(), &value, sizeof(value));
//memcpy(&param_getset_req[_driver_index].value.string_value.c_str(), &value, sizeof(value));
//param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::string_value>() = 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
*/
bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb)
{
WITH_SEMAPHORE(_param_sem);

// 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[_driver_index].index = 0;
Expand All @@ -1730,12 +1760,15 @@ bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGe
return true;
}

/*
get named integer parameter on node
*/
bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb)
{
WITH_SEMAPHORE(_param_sem);
if (param_int_cb != nullptr ||
param_float_cb != nullptr) {
//busy
param_float_cb != nullptr ||
param_string_cb != nullptr) {
return false;
}
param_getset_req[_driver_index].index = 0;
Expand All @@ -1748,11 +1781,35 @@ bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGe
return true;
}

/*
get named string parameter on node
*/
bool AP_UAVCAN::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[_driver_index].index = 0;
param_getset_req[_driver_index].name = name;
param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::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_UAVCAN::handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb)
{
WITH_SEMAPHORE(ap_uavcan->_param_sem);
if (!ap_uavcan->param_int_cb &&
!ap_uavcan->param_float_cb) {
!ap_uavcan->param_float_cb &&
!ap_uavcan->param_string_cb) {
return;
}
uavcan::protocol::param::GetSet::Response rsp = cb.rsp->getResponse();
Expand Down Expand Up @@ -1782,10 +1839,25 @@ void AP_UAVCAN::handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node
ap_uavcan->param_request_node_id = node_id;
return;
}
} else if ((rsp.value.is(uavcan::protocol::param::Value::Tag::string_value)) && ap_uavcan->param_string_cb) {
string val;
memcpy(&val, &rsp.value.string_value, sizeof(val));
if ((*ap_uavcan->param_string_cb)(ap_uavcan, node_id, rsp.name.c_str(), val)) {
// we want the parameter to be set with val
param_getset_req[ap_uavcan->_driver_index].index = 0;
param_getset_req[ap_uavcan->_driver_index].name = rsp.name;
memcpy(&param_getset_req[ap_uavcan->_driver_index].value.string_value, &val, sizeof(val));
ap_uavcan->param_string_cb = ap_uavcan->param_string_cb;
ap_uavcan->param_request_sent = false;
ap_uavcan->param_request_sent_ms = AP_HAL::millis();
ap_uavcan->param_request_node_id = node_id;
return;
}
}
ap_uavcan->param_request_sent_ms = 0;
ap_uavcan->param_int_cb = nullptr;
ap_uavcan->param_float_cb = nullptr;
ap_uavcan->param_string_cb = nullptr;
}


Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_UAVCAN/AP_UAVCAN.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,8 +147,12 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
uavcan::Node<0>* get_node() { return _node; }
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_UAVCAN*, const uint8_t, const char*, int32_t &);
FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
FUNCTOR_TYPEDEF(ParamGetSetStringCb, bool, AP_UAVCAN*, const uint8_t, const char*, string &);
FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_UAVCAN*, const uint8_t, bool);

///// SRV output /////
Expand Down Expand Up @@ -180,8 +184,10 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
// 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);
Expand Down Expand Up @@ -302,6 +308,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
// 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
Expand Down

0 comments on commit 3716e34

Please sign in to comment.