Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Cyphal: optimize ESC setpoint

Browse Source
This commit is contained in:
PonomarevDA
2023-04-24 08:17:40 +03:00
committed by Daniel Agar
parent 41bd6c92e2
commit f81e36a3a0
1 changed files with 34 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/cyphal/Actuators/EscClient.hpp
+24 -27
View File
@@ -166,53 +166,48 @@ public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
UavcanPublisher(handle, pmgr, "udral.", "esc") { }; UavcanPublisher(handle, pmgr, "udral.", "esc") { }
~UavcanEscController() {}; ~UavcanEscController() {}
void update() override void update() override
{ {
}; }
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{ {
if (_port_id > 0 && _port_id != CANARD_PORT_ID_UNSET) { if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; return;
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++) { uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS;
if (i < num_outputs) {
msg_sp.value[i] = static_cast<float>(outputs[i] / 8192.0);
} else { for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) {
// "unset" values published as NaN if (outputs[i] != 0) {
msg_sp.value[i] = NAN; _max_number_of_nonzero_outputs = i + 1;
break;
} }
} }
uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_];
for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) {
payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0);
}
const CanardTransferMetadata transfer_metadata = { const CanardTransferMetadata transfer_metadata = {
.priority = CanardPriorityNominal, .priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage, .transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID. .port_id = _port_id,
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id, .transfer_id = _transfer_id,
}; };
int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, ++_transfer_id;
&payload_size); _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_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, &transfer_metadata,
payload_size, _max_number_of_nonzero_outputs * 2,
&esc_sp_payload_buffer); &payload_buffer);
} }
}
};
/** /**
* Sets the number of rotors * Sets the number of rotors
@@ -225,7 +220,9 @@ private:
*/ */
void esc_status_sub_cb(const CanardRxTransfer &msg); void esc_status_sub_cb(const CanardRxTransfer &msg);
uint8_t _max_number_of_nonzero_outputs{1};
uint8_t _rotor_count {0}; uint8_t _rotor_count {0};
orb_advert_t _mavlink_log_pub{nullptr};
}; };
class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber
@@ -268,7 +265,7 @@ public:
_esc_status.counter += 1; _esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = 7; // check_escs_status(); _esc_status.esc_online_flags = 7; // check_escs_status();
_esc_status.esc_armed_flags = 7 ; // (1 << _rotor_count) - 1; _esc_status.esc_armed_flags = 7; // (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time(); _esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.publish(_esc_status); _esc_status_pub.publish(_esc_status);
} }