drivers/cyphal: incremental fixes for fmu-v5 (#20671)

* Cyphal: fix comparing floating-point issue

* Cyphal: fix setpoint serialization

* Cyphal: fix bug with wrong comparasion of param name and pub/sub name: remove prefix from UavcanPublisher::updateParam and UavcanDynamicPortSubscriber::updateParam and PublicationManager::updateDynamicPublications

* Cyphal: integrate UavcanEscController with PublicationManager, remove second instance of UavcanEscController from CyphalNode

* Cyphal: publish readiness with minimal frequency because according to UDRAL The drive shall enter STANDBY state automatically if the readiness subject is not updated for CONTROL_TIMEOUT

* Cyphal: increase setpoint publish rate from ~75 to 200 by removing PX4_INFO (it really significantly react on the the output rate) and changing the mixing output rate and the shedule interval

* Cyphal: restore prefix because we need it for uorb over uavcan/cyphal and add udral prefix for non uorb pub/sub

* Cyphal: fix DynamicPortSubscriber subscription: if it has multiple subscribers, it should call subscription only after updating of all port subscribers port identifiers

* Cyphal: fix SubscriptionManager: we should take care about prefix

* Cyphal: fix readiness for test motor mode

* [Cyphal] Fix dynamicsubscription, improve printinfo, enable MR-CANHUBK3 config

---------

Co-authored-by: Peter van der Perk <peter.vanderperk@nxp.com>
This commit is contained in:
Dmitry Ponomarev
2023-02-23 10:57:50 -05:00
committed by GitHub
co-authored by Peter van der Perk
parent 013365d6c8
commit a1efafc42b
15 changed files with 163 additions and 93 deletions
+12 -3
View File
@@ -83,6 +83,8 @@ CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
_pub_manager.updateParams();
_sub_manager.subscribe();
_mixing_output.mixingOutput().setMaxTopicUpdateRate(1000000 / 200);
}
CyphalNode::~CyphalNode()
@@ -135,6 +137,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
_instance->active_bitrate = bitrate;
_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
_instance->_mixing_output.ScheduleNow();
return PX4_OK;
}
@@ -254,17 +257,19 @@ void CyphalNode::print_info()
_pub_manager.printInfo();
PX4_INFO("Message subscriptions:");
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindMessage),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
PX4_INFO("Message port id %d", sub->port_id);
} else {
((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
((UavcanBaseSubscriber *)sub->user_reference)->printInfo(sub->port_id);
}
});
PX4_INFO("Service response subscriptions:");
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindRequest),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
@@ -275,6 +280,7 @@ void CyphalNode::print_info()
}
});
PX4_INFO("Service request subscriptions:");
traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindResponse),
[&](const CanardRxSubscription * const sub) {
if (sub->user_reference == nullptr) {
@@ -393,8 +399,11 @@ bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX
// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect
/// TODO: Need esc/servo metadata / bitmask(s)
_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
auto publisher = static_cast<UavcanEscController *>(_pub_manager.getPublisher("esc"));
if (publisher) {
publisher->update_outputs(stop_motors, outputs, num_outputs);
}
return true;
}