fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 02:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/cyphal/Actuators/EscClient.hpp
+68 -48
View File
@@ -53,6 +53,7 @@
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/esc_status.h>
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
@@ -69,69 +70,38 @@ public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral", "esc") { };
UavcanPublisher(handle, pmgr, "udral.", "esc") { };
~UavcanEscController() {};
void update() override
{
actuator_test_s actuator_test;
if (_actuator_test_sub.update(&actuator_test)) {
_actuator_test_timestamp = actuator_test.timestamp;
}
if (_armed_sub.updated()) {
actuator_armed_s new_arming;
_armed_sub.update(&new_arming);
if (new_arming.armed != _armed.armed) {
_armed = new_arming;
size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
// Only publish if we have a valid publication ID set
if (_port_id == 0) {
return;
}
reg_udral_service_common_Readiness_0_1 msg_arming {};
if (_armed.armed) {
msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
} else if (_armed.prearmed) {
msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
} else {
msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
}
uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = arming_pid, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _arming_transfer_id,
};
int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&arming_payload_buffer
);
}
publish_readiness();
}
}
if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
publish_readiness();
}
};
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if (_port_id > 0) {
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
if (i < num_outputs) {
@@ -143,10 +113,6 @@ public:
}
}
PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
(double)msg_sp.value[2], (double)msg_sp.value[3]);
uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
const CanardTransferMetadata transfer_metadata = {
@@ -182,10 +148,64 @@ private:
*/
void esc_status_sub_cb(const CanardRxTransfer &msg);
void publish_readiness()
{
const hrt_abstime now = hrt_absolute_time();
_previous_pub_time = now;
size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
// Only publish if we have a valid publication ID set
if (_port_id == 0) {
return;
}
reg_udral_service_common_Readiness_0_1 msg_arming {};
if (_armed.armed || _actuator_test_timestamp + 100_ms > now) {
msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
} else if (_armed.prearmed) {
msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
} else {
msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
}
uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = arming_pid, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _arming_transfer_id,
};
int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
&payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
&transfer_metadata,
payload_size,
&arming_payload_buffer);
}
};
uint8_t _rotor_count {0};
static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
hrt_abstime _previous_pub_time = 0;
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
actuator_armed_s _armed {};
uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
uint64_t _actuator_test_timestamp{0};
CanardTransferID _arming_transfer_id;
};