Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: add support for setting parameters on CAN nodes

Browse Source
This commit is contained in:
bugobliterator 2021-07-16 21:57:11 +05:30 committed by Andrew Tridgell
parent 4f65705c99
commit e8c4b99a99
2 changed files with 264 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

185
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -115,6 +115,16 @@ static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_M
static uavcan::Publisher<uavcan::equipment::safety::ArmingStatus>* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// Clients
UC_CLIENT_CALL_REGISTRY_BINDER(ParamGetSetCb, uavcan::protocol::param::GetSet);
static uavcan::ServiceClient<uavcan::protocol::param::GetSet, ParamGetSetCb>* param_get_set_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::protocol::param::GetSet::Request param_getset_req[HAL_MAX_CAN_PROTOCOL_DRIVERS];
UC_CLIENT_CALL_REGISTRY_BINDER(ParamExecuteOpcodeCb, uavcan::protocol::param::ExecuteOpcode);
static uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecuteOpcodeCb>* param_execute_opcode_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::protocol::param::ExecuteOpcode::Request param_save_req[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// subscribers
// handler SafteyButton
@ -294,6 +304,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
param_get_set_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::GetSet, ParamGetSetCb>(*_node, ParamGetSetCb(this, &AP_UAVCAN::handle_param_get_set_response));
param_execute_opcode_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecuteOpcodeCb>(*_node, ParamExecuteOpcodeCb(this, &AP_UAVCAN::handle_param_save_response));
safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
if (safety_button_listener[driver_index]) {
safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@ -392,6 +406,8 @@ void AP_UAVCAN::loop(void)
buzzer_send();
rtcm_stream_send();
safety_state_send();
send_parameter_request();
send_parameter_save_request();
AP::uavcan_dna_server().verify_nodes(this);
}
}
@ -835,4 +851,173 @@ void AP_UAVCAN::handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugC
#endif
}
void AP_UAVCAN::send_parameter_request()
{
WITH_SEMAPHORE(_param_sem);
if (param_request_sent) {
return;
}
param_get_set_client[_driver_index]->call(param_request_node_id, param_getset_req[_driver_index]);
param_request_sent = true;
}
bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb)
{
WITH_SEMAPHORE(_param_sem);
if (param_int_cb != nullptr ||
param_float_cb != nullptr) {
//busy
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::real_value>() = value;
param_float_cb = cb;
param_request_sent = false;
param_request_node_id = node_id;
return true;
}
bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb)
{
WITH_SEMAPHORE(_param_sem);
if (param_int_cb != nullptr ||
param_float_cb != nullptr) {
//busy
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::integer_value>() = value;
param_int_cb = cb;
param_request_sent = false;
param_request_node_id = node_id;
return true;
}
bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb)
{
WITH_SEMAPHORE(_param_sem);
if (param_int_cb != nullptr ||
param_float_cb != nullptr) {
//busy
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_float_cb = cb;
param_request_sent = false;
param_request_node_id = node_id;
return true;
}
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
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_int_cb = cb;
param_request_sent = false;
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) {
return;
}
uavcan::protocol::param::GetSet::Response rsp = cb.rsp->getResponse();
if (rsp.value.is(uavcan::protocol::param::Value::Tag::integer_value) && ap_uavcan->param_int_cb) {
int32_t val = rsp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
if ((*ap_uavcan->param_int_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;
param_getset_req[ap_uavcan->_driver_index].value.to<uavcan::protocol::param::Value::Tag::integer_value>() = val;
ap_uavcan->param_int_cb = ap_uavcan->param_int_cb;
ap_uavcan->param_request_sent = false;
ap_uavcan->param_request_node_id = node_id;
return;
}
} else if (rsp.value.is(uavcan::protocol::param::Value::Tag::real_value) && ap_uavcan->param_float_cb) {
float val = rsp.value.to<uavcan::protocol::param::Value::Tag::real_value>();
if ((*ap_uavcan->param_float_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;
param_getset_req[ap_uavcan->_driver_index].value.to<uavcan::protocol::param::Value::Tag::real_value>() = val;
ap_uavcan->param_float_cb = ap_uavcan->param_float_cb;
ap_uavcan->param_request_sent = false;
ap_uavcan->param_request_node_id = node_id;
return;
}
}
ap_uavcan->param_int_cb = nullptr;
ap_uavcan->param_float_cb = nullptr;
}
void AP_UAVCAN::send_parameter_save_request()
{
WITH_SEMAPHORE(_param_save_sem);
if (param_save_request_sent) {
return;
}
param_execute_opcode_client[_driver_index]->call(param_save_request_node_id, param_save_req[_driver_index]);
param_save_request_sent = true;
}
bool AP_UAVCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb)
{
WITH_SEMAPHORE(_param_save_sem);
if (save_param_cb != nullptr) {
//busy
return false;
}
param_save_req[_driver_index].opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
param_save_request_sent = false;
param_save_request_node_id = node_id;
save_param_cb = cb;
return true;
}
// handle parameter save request response
void AP_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb)
{
WITH_SEMAPHORE(ap_uavcan->_param_save_sem);
if (!ap_uavcan->save_param_cb) {
return;
}
uavcan::protocol::param::ExecuteOpcode::Response rsp = cb.rsp->getResponse();
(*ap_uavcan->save_param_cb)(ap_uavcan, node_id, rsp.ok);
ap_uavcan->save_param_cb = nullptr;
}
// Send Reboot command
// Note: Do not call this from outside UAVCAN thread context,
// THIS IS NOT A THREAD SAFE API!
void AP_UAVCAN::send_reboot_request(uint8_t node_id)
{
if (_node == nullptr) {
return;
}
uavcan::protocol::RestartNode::Request request;
request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER;
uavcan::ServiceClient<uavcan::protocol::RestartNode> client(*_node);
client.setCallback([](const uavcan::ServiceCallResult<uavcan::protocol::RestartNode>& call_result){});
client.call(node_id, request);
}
#endif // HAL_NUM_CAN_IFACES

80
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -28,7 +28,8 @@
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
@ -58,6 +59,8 @@ class TrafficReportCb;
class ActuatorStatusCb;
class ESCStatusCb;
class DebugCb;
class ParamGetSetCb;
class ParamExecuteOpcodeCb;
#if defined(__GNUC__) && (__GNUC__ > 8)
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
@ -85,6 +88,17 @@ class DebugCb;
DISABLE_W_CAST_FUNCTION_TYPE_POP \
}
#define UC_CLIENT_CALL_REGISTRY_BINDER(ClassName_, DataType_) \
class ClassName_ : public AP_UAVCAN::ClientCallRegistryBinder<DataType_> { \
typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
public: \
ClassName_() : ClientCallRegistryBinder() {} \
DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
ClientCallRegistryBinder(uc, (ClientCallRegistry)ffunc) {} \
DISABLE_W_CAST_FUNCTION_TYPE_POP \
}
class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
public:
AP_UAVCAN();
@ -101,6 +115,9 @@ public:
uavcan::Node<0>* get_node() { return _node; }
uint8_t get_driver_index() const { return _driver_index; }
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(ParamSaveCb, void, AP_UAVCAN*, const uint8_t, bool);
///// SRV output /////
void SRV_push_servos(void);
@ -114,6 +131,21 @@ public:
// send RTCMStream packets
void send_RTCMStream(const uint8_t *data, uint32_t len);
// Send Reboot command
// Note: Do not call this from outside UAVCAN thread context,
// you can call this from uavcan callbacks and handlers.
// THIS IS NOT A THREAD SAFE API!
void send_reboot_request(uint8_t node_id);
// set param value
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 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);
// Save parameters
bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb);
template <typename DataType_>
class RegistryBinder {
protected:
@ -140,6 +172,31 @@ public:
const uavcan::ReceivedDataStructure<DataType_> *msg;
};
// ClientCallRegistryBinder
template <typename DataType_>
class ClientCallRegistryBinder {
protected:
typedef void (*ClientCallRegistry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const ClientCallRegistryBinder& _cb);
AP_UAVCAN* _uc;
ClientCallRegistry _ffunc;
public:
ClientCallRegistryBinder() :
_uc(),
_ffunc(),
rsp() {}
ClientCallRegistryBinder(AP_UAVCAN* uc, ClientCallRegistry ffunc) :
_uc(uc),
_ffunc(ffunc),
rsp(nullptr) {}
void operator()(const uavcan::ServiceCallResult<DataType_>& _rsp) {
rsp = &_rsp;
_ffunc(_uc, _rsp.getCallID().server_node_id.get(), *this);
}
const uavcan::ServiceCallResult<DataType_> *rsp;
};
private:
// This will be needed to implement if UAVCAN is used with multithreading
// Such cases will be firmware update, etc.
@ -163,6 +220,25 @@ private:
// send GNSS injection
void rtcm_stream_send();
// send parameter get/set request
void send_parameter_request();
// send parameter save request
void send_parameter_save_request();
// set parameter on a node
ParamGetSetIntCb *param_int_cb;
ParamGetSetFloatCb *param_float_cb;
bool param_request_sent = true;
HAL_Semaphore _param_sem;
uint8_t param_request_node_id;
// save parameters on a node
ParamSaveCb *save_param_cb;
bool param_save_request_sent = true;
HAL_Semaphore _param_save_sem;
uint8_t param_save_request_node_id;
uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
// UAVCAN parameters
@ -234,6 +310,8 @@ private:
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
static bool is_esc_data_index_valid(const uint8_t index);
static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
};
#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS