fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 09:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Add to EscClient implementation

Browse Source 
Add outline of EscServer implementation
Add some comments and cleanup
This commit is contained in:
JacobCrabill
2021-02-24 19:22:11 +01:00
committed by Lorenz Meier
parent 4b73566b76
commit 04ea1cf5c6
4 changed files with 159 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Actuators/EscClient.hpp
+78 -21
View File
@@ -32,46 +32,101 @@
****************************************************************************/
/**
* @file esc.hpp
* @file EscClient.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand
* uavcan.equipment.esc.RPMCommand
* uavcan.equipment.esc.Status
* Client-side implementation of DS-15 specification ESC service
*
* Publishes the following UAVCAN v1 messages:
* reg.drone.service.actuator.common.sp.Value8.0.1
* reg.drone.service.common.Readiness.0.1
*
* Subscribes to the following UAVCAN v1 messages:
* reg.drone.service.actuator.common.Feedback.0.1
* reg.drone.service.actuator.common.Status.0.1
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
// DS-15 Specification Messages
#include <reg/drone/service/actuator/common/sp/Vector31_0_1.h>
#include <reg/drone/service/common/Readiness_0_1.h>
class UavcanEscController
/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
class UavcanEscController : public UavcanPublication
{
public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;
/// TODO: derive from Publisher class to handle port-ID setting via parameter
UavcanEscController(CanardInstance &ins, const CanardPortID &baseID) :
_canard_instance(ins), _base_port_id(baseID) {};
UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanPublication(ins, pmgr, uavcan_pname) { };
~UavcanEscController() {};
void update() override
{
if (_armed_sub.updated()) {
actuator_armed_s new_arming;
_armed_sub.update(&new_arming);
if (new_arming.armed != _armed.armed) {
_armed = new_arming;
// Only publish if we have a valid publication ID set
if (_port_id == 0) {
return;
}
reg_drone_service_common_Readiness_0_1 msg_arming {};
if (_armed.armed) {
msg_arming.value = reg_drone_service_common_Readiness_0_1_ENGAGED;
} else if (_armed.prearmed) {
msg_arming.value = reg_drone_service_common_Readiness_0_1_STANDBY;
} else {
msg_arming.value = reg_drone_service_common_Readiness_0_1_SLEEP;
}
uint8_t arming_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.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,
.payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &arming_payload_buffer,
};
int result = reg_drone_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
&transfer.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 = canardTxPush(&_canard_instance, &transfer);
}
}
}
};
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if (_base_port_id > 0) {
/// TODO: Update readiness
// reg_drone_service_common_Readiness_0_1 msg_arming;
if (_port_id > 0) {
reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
for (uint8_t i = 0; i < num_outputs; i++) {
@@ -84,9 +139,9 @@ public:
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _base_port_id, // This is the subject-ID.
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = _esc_setpoint_transfer_id,
.transfer_id = _transfer_id,
.payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &esc_sp_payload_buffer,
};
@@ -96,7 +151,7 @@ public:
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_esc_setpoint_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
@@ -113,8 +168,10 @@ private:
*/
void esc_status_sub_cb(const CanardTransfer &msg);
CanardInstance &_canard_instance;
CanardPortID _base_port_id {0};
uint8_t _rotor_count {0};
CanardTransferID _esc_setpoint_transfer_id {0};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
actuator_armed_s _armed {};
CanardTransferID _arming_transfer_id;
};