diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 777fe9f3563cec..19de2819044e15 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -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; } } @@ -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; @@ -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; @@ -1709,7 +1712,33 @@ 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(¶m_getset_req[_driver_index].value.c_str(), &value, sizeof(value)); + //memcpy(¶m_getset_req[_driver_index].value.to(), &value, sizeof(value)); + //memcpy(¶m_getset_req[_driver_index].value.string_value.c_str(), &value, sizeof(value)); + //param_getset_req[_driver_index].value.to() = 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) { @@ -1717,7 +1746,8 @@ bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGe // 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; @@ -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; @@ -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(); + 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(); @@ -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(¶m_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; } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index baf6ed8aa20e9e..668a7b8e511f43 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -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 ///// @@ -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); @@ -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