fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cyphal: divide EscClient into 2 publishers, so setpoint and readiness are 2 different ports now

Browse Source
This commit is contained in:
PonomarevDA 2023-04-24 00:11:39 +03:00 committed by Daniel Agar
parent 52476633a8
commit 515543b1c5
4 changed files with 100 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
src/drivers/cyphal/Actuators/EscClient.hpp
View File

@ -63,16 +63,14 @@ using std::isfinite;
#include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h>
/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
class UavcanEscController : public UavcanPublisher
class ReadinessPublisher : public UavcanPublisher
{
public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral.", "esc") { };
ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral.", "readiness") { };
~UavcanEscController() {};
~ReadinessPublisher() {};
void update() override
{
@ -95,11 +93,86 @@ public:
if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
publish_readiness();
}
}
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;
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 || _port_id == CANARD_PORT_ID_UNSET) {
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));
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);
}
};
};
/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
class UavcanEscController : public UavcanPublisher
{
public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral.", "esc") { };
~UavcanEscController() {};
void update() override
{
};
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if (_port_id > 0) {
if (_port_id > 0 && _port_id != CANARD_PORT_ID_UNSET) {
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
@ -148,64 +221,5 @@ 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;
};

3
src/drivers/cyphal/ParamManager.hpp
View File

@ -116,8 +116,9 @@ public:
private:
const UavcanParamBinder _uavcan_params[13] {
const UavcanParamBinder _uavcan_params[14] {
{"uavcan.pub.udral.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.udral.readiness.0.id", "UCAN1_READ_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.udral.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.udral.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
{"uavcan.pub.udral.actuator_outputs.0.id", "UCAN1_ACTR_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},

10
src/drivers/cyphal/PublicationManager.hpp
View File

@ -67,7 +67,7 @@
/* Preprocessor calculation of publisher count */
#define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
CONFIG_CYPHAL_ESC_CONTROLLER + \
2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
CONFIG_CYPHAL_READINESS_PUBLISHER + \
CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@ -133,6 +133,14 @@ private:
"udral.esc",
0
},
{
[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
{
return new ReadinessPublisher(handle, pmgr);
},
"udral.readiness",
0
},
#endif
#if CONFIG_CYPHAL_READINESS_PUBLISHER
{

9
src/drivers/cyphal/parameters.c
View File

@ -162,6 +162,15 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
*/
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
/**
* Cyphal ESC readiness port ID.
*
* @min -1
* @max 6143
* @group Cyphal
*/
PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
/**
* Cyphal GPS publication port ID.
*