yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00: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-22 08:17:34 -08: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

99
src/drivers/uavcan_v1/Actuators/EscClient.hpp
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;
};

74
src/drivers/uavcan_v1/Actuators/EscServer.hpp Normal file
View File

@@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file EscServer.hpp
*
* Server-side implementation of DS-15 specification ESC service
* (Used for CANNode control of an ESC via PWM output)
*
* Subscribes to the following UAVCAN v1 messages:
* reg.drone.service.actuator.common.sp.Value8.0.1
* reg.drone.service.common.Readiness.0.1
*
* Publishes to the following UAVCAN v1 messages:
* reg.drone.service.actuator.common.Feedback.0.1
* reg.drone.service.actuator.common.Status.0.1
*
* And publishes to the following uORB topics:
* actuator_armed
* output_control_mc
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#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>
#include "../Publishers/Publisher.hpp"
class EscServer : public UavcanSubscription
{
/// TODO
}

1
src/drivers/uavcan_v1/Uavcan.cpp
View File

@@ -403,6 +403,7 @@ void UavcanNode::print_info()
}
_mixing_output.printInfo();
_esc_controller.printInfo();
pthread_mutex_unlock(&_node_mutex);
}

9
src/drivers/uavcan_v1/Uavcan.hpp
View File

@@ -237,7 +237,11 @@ private:
UavcanGnssPublication _gps_pub {_canard_instance, _param_manager, "uavcan.pub.gps.0.id"};
UavcanPublication *_publishers[1] {&_gps_pub};
UavcanEscController _esc_controller {_canard_instance, _param_manager, "uavcan.pub.esc.0.id"};
// Publication objects: Any object used to bridge a uORB message to a UAVCAN message
/// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers
UavcanPublication *_publishers[2] {&_gps_pub, &_esc_controller};
UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"};
UavcanGnssSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"};
@@ -245,9 +249,8 @@ private:
UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"};
UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"};
// Subscription objects: Any object used to bridge a UAVCAN message to a uORB message
UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanEscController _esc_controller {_canard_instance, 22}; //// TODO
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
};