forked from Archive/PX4-Autopilot
uavcan_v1: Fix SubscriptionManager dynamic update
This commit is contained in:
parent
db460daf46
commit
a99b657f03
|
@ -47,23 +47,31 @@
|
|||
#include <uavcan/_register/Name_1_0.h>
|
||||
#include <uavcan/_register/Value_1_0.h>
|
||||
|
||||
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
||||
static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U;
|
||||
|
||||
static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
|
||||
{
|
||||
if (param_type(in) == PARAM_TYPE_INT32) {
|
||||
int32_t out_val {};
|
||||
param_get(in, &out_val);
|
||||
const int res = param_get(in, &out_val);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
// Parameter not found / internal error
|
||||
return false;
|
||||
}
|
||||
|
||||
if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) {
|
||||
out.natural16.value.elements[0] = (uint16_t)out_val;
|
||||
out.natural16.value.count = 1;
|
||||
uavcan_register_Value_1_0_select_natural16_(&out);
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
// "Invalid" value -- set to "UNSET"
|
||||
out.natural16.value.elements[0] = CANARD_PORT_ID_UNSET;
|
||||
}
|
||||
|
||||
out.natural16.value.count = 1;
|
||||
uavcan_register_Value_1_0_select_natural16_(&out);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
|
|
@ -59,8 +59,7 @@ public:
|
|||
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||
virtual void update() override
|
||||
{
|
||||
if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET
|
||||
&& _port_id != 0) { //FIXME either make default param UNSET or handle 0 in base class
|
||||
if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
||||
sensor_gps_s gps {};
|
||||
_gps_sub.update(&gps);
|
||||
|
||||
|
|
|
@ -60,7 +60,7 @@ public:
|
|||
virtual void update() override
|
||||
{
|
||||
// Not sure if actuator_armed is a good indication of readiness but seems close to it
|
||||
if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
|
||||
if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
||||
actuator_armed_s armed {};
|
||||
_actuator_armed_sub.update(&armed);
|
||||
|
||||
|
|
|
@ -56,7 +56,7 @@ public:
|
|||
virtual void update() override
|
||||
{
|
||||
// Not sure if actuator_armed is a good indication of readiness but seems close to it
|
||||
if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
|
||||
if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
||||
sensor_gps_s gps_msg {};
|
||||
_sensor_gps_sub.update(&gps_msg);
|
||||
|
||||
|
|
|
@ -61,26 +61,44 @@ void SubscriptionManager::subscribe()
|
|||
_access_rsp.subscribe();
|
||||
|
||||
for (auto &sub : _uavcan_subs) {
|
||||
param_t param_handle = param_find(sub.px4_name);
|
||||
if (sub.instance == NULL) {
|
||||
param_t param_handle = param_find(sub.px4_name);
|
||||
|
||||
if (param_handle == PARAM_INVALID) {
|
||||
PX4_ERR("Param %s not found", sub.px4_name);
|
||||
break;
|
||||
}
|
||||
if (param_handle == PARAM_INVALID) {
|
||||
PX4_ERR("Param %s not found", sub.px4_name);
|
||||
break;
|
||||
}
|
||||
|
||||
if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
|
||||
int32_t port_id {};
|
||||
param_get(param_handle, &port_id);
|
||||
if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
|
||||
int32_t port_id {};
|
||||
param_get(param_handle, &port_id);
|
||||
|
||||
if (port_id >= 0) { // PortID is set create a subscriber
|
||||
UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
|
||||
if (port_id >= 0) { // PortID is set create a subscriber
|
||||
UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
|
||||
|
||||
if (_dynsubscribers != NULL) {
|
||||
_dynsubscribers->setNext(dynsub);
|
||||
if (dynsub == NULL) {
|
||||
PX4_ERR("Out of memory");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_dynsubscribers == NULL) {
|
||||
// Set the head of our linked list
|
||||
_dynsubscribers = dynsub;
|
||||
|
||||
} else {
|
||||
// Append the new subscriber to our linked list
|
||||
UavcanDynamicPortSubscriber *tmp = _dynsubscribers;
|
||||
|
||||
while (tmp->next() != NULL) {
|
||||
tmp = tmp->next();
|
||||
}
|
||||
|
||||
tmp->setNext(dynsub);
|
||||
}
|
||||
|
||||
sub.instance = dynsub;
|
||||
dynsub->updateParam();
|
||||
}
|
||||
|
||||
_dynsubscribers = dynsub;
|
||||
dynsub->updateParam();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -104,4 +122,7 @@ void SubscriptionManager::updateParams()
|
|||
dynsub->updateParam();
|
||||
dynsub = dynsub->next();
|
||||
}
|
||||
|
||||
// Check for any newly-enabled subscriptions
|
||||
subscribe();
|
||||
}
|
||||
|
|
|
@ -60,6 +60,7 @@
|
|||
typedef struct {
|
||||
const char *px4_name;
|
||||
UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
|
||||
UavcanDynamicPortSubscriber *instance {nullptr};
|
||||
} UavcanDynSubBinder;
|
||||
|
||||
class SubscriptionManager
|
||||
|
@ -85,48 +86,54 @@ private:
|
|||
// Process register requests
|
||||
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
|
||||
|
||||
const UavcanDynSubBinder _uavcan_subs[6] {
|
||||
UavcanDynSubBinder _uavcan_subs[6] {
|
||||
{
|
||||
"UCAN1_ESC0_PID",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UavcanEscSubscriber(ins, pmgr, 0);
|
||||
}
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
{
|
||||
"UCAN1_GPS0_PID",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UavcanGnssSubscriber(ins, pmgr, 1);
|
||||
}
|
||||
return new UavcanGnssSubscriber(ins, pmgr, 0);
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
{
|
||||
"UCAN1_GPS1_PID",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UavcanGnssSubscriber(ins, pmgr, 0);
|
||||
}
|
||||
return new UavcanGnssSubscriber(ins, pmgr, 1);
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
{
|
||||
"UCAN1_BMS_ES_PID",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UavcanBmsSubscriber(ins, pmgr, 0);
|
||||
}
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
{
|
||||
"UCAN1_LG_BMS_PID",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
|
||||
}
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
{
|
||||
"UCAN1_UORB_GPS",
|
||||
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
|
||||
{
|
||||
return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
|
||||
}
|
||||
},
|
||||
nullptr
|
||||
},
|
||||
};
|
||||
};
|
||||
|
|
|
@ -364,7 +364,6 @@ void UavcanNode::print_info()
|
|||
}
|
||||
|
||||
_mixing_output.printInfo();
|
||||
_esc_controller.printInfo();
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue