mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-22 03:23:18 +08:00
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:
co-authored by
Peter van der Perk
parent
013365d6c8
commit
a1efafc42b
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user