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

uavcan module cleanup

Browse Source 
- move most orb to uORB::Publication and uORB::Subscription
 - update legacy message handling (warn to PX4_INFO, PX4_WARN, PX4_ERR)
 - add perf counters
 - sensors/mag support newer `uavcan::equipment::ahrs::MagneticFieldStrength2` message
 - sensors/gps support `uavcan::equipment::gnss::Auxiliary` for hdop and vdop
 - sensors delete obsolete ioctl and read methods
 - use PublicationMulti for actuator_outputs and esc_reports (to coexist with other output modules)
 - add GNSS parameter metadata (parameters_injected.xml)
This commit is contained in:
Daniel Agar
2019-10-28 19:57:50 -04:00
committed by GitHub
parent 47dd312b57
commit 375fc4a75c
22 changed files with 705 additions and 677 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/actuators/esc.cpp
+22 -35
View File
@@ -52,14 +52,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
_orb_timer(node)
{
_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
if (_perfcnt_invalid_input == nullptr) {
errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
}
if (_perfcnt_scaling_error == nullptr) {
errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
}
}
UavcanEscController::~UavcanEscController()
@@ -68,13 +60,14 @@ UavcanEscController::~UavcanEscController()
perf_free(_perfcnt_scaling_error);
}
int UavcanEscController::init()
int
UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0) {
warnx("ESC status sub failed %i", res);
PX4_ERR("ESC status sub failed %i", res);
return res;
}
@@ -85,11 +78,13 @@ int UavcanEscController::init()
return res;
}
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
void
UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -111,9 +106,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
*/
uavcan::equipment::esc::RawCommand msg;
actuator_outputs_s actuator_outputs = {};
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = num_outputs;
actuator_outputs.timestamp = hrt_absolute_time();
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F;
@@ -167,21 +161,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* Publish the command message to the bus
* Note that for a quadrotor it takes one CAN frame
*/
(void)_uavcan_pub_raw_cmd.broadcast(msg);
_uavcan_pub_raw_cmd.broadcast(msg);
// Publish actuator outputs
if (_actuator_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
} else {
int instance;
_actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs,
&instance, ORB_PRIO_DEFAULT);
}
actuator_outputs.timestamp = hrt_absolute_time();
_actuator_outputs_pub.publish(actuator_outputs);
}
void UavcanEscController::arm_all_escs(bool arm)
void
UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
@@ -191,7 +179,8 @@ void UavcanEscController::arm_all_escs(bool arm)
}
}
void UavcanEscController::arm_single_esc(int num, bool arm)
void
UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
@@ -201,7 +190,8 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
}
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
void
UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
auto &ref = _esc_status.esc[msg.esc_index];
@@ -217,7 +207,8 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
void
UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.timestamp = hrt_absolute_time();
_esc_status.esc_count = _rotor_count;
@@ -225,18 +216,14 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
if (_esc_status_pub != nullptr) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
_esc_status_pub.publish(_esc_status);
}
uint8_t UavcanEscController::check_escs_status()
uint8_t
UavcanEscController::check_escs_status()
{
int esc_status_flags = 0;
hrt_abstime now = hrt_absolute_time();
const hrt_abstime now = hrt_absolute_time();
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
src/drivers/uavcan/actuators/esc.hpp
+18 -16
View File
@@ -47,12 +47,12 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <perf/perf_counter.h>
#include <uORB/topics/esc_status.h>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/esc_status.h>
#include <drivers/drv_hrt.h>
class UavcanEscController
{
public:
@@ -95,17 +95,19 @@ private:
static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
TimerCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
bool _armed{false};
bool _run_at_idle_throttle_when_armed{false};
esc_status_s _esc_status{};
uORB::PublicationMulti<actuator_outputs_s> _actuator_outputs_pub{ORB_ID(actuator_outputs)};
uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
bool _armed = false;
bool _run_at_idle_throttle_when_armed = false;
esc_status_s _esc_status = {};
orb_advert_t _esc_status_pub = nullptr;
orb_advert_t _actuator_outputs_pub = nullptr;
uint8_t _rotor_count = 0;
/*
@@ -120,12 +122,12 @@ private:
/*
* ESC states
*/
uint32_t _armed_mask = 0;
uint8_t _max_number_of_nonzero_outputs = 0;
uint32_t _armed_mask{0};
uint8_t _max_number_of_nonzero_outputs{0};
/*
* Perf counters
*/
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
perf_counter_t _perfcnt_invalid_input{perf_alloc(PC_COUNT, "uavcan_esc_invalid_input")};
perf_counter_t _perfcnt_scaling_error{perf_alloc(PC_COUNT, "uavcan_esc_scaling_error")};
};
src/drivers/uavcan/actuators/hardpoint.cpp
+10 -9
View File
@@ -48,13 +48,13 @@ UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
}
UavcanHardpointController::~UavcanHardpointController()
{
}
int UavcanHardpointController::init()
int
UavcanHardpointController::init()
{
/*
* Setup timer and call back function for periodic updates
@@ -63,7 +63,8 @@ int UavcanHardpointController::init()
return 0;
}
void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
void
UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
{
_cmd.command = command;
_cmd.hardpoint_id = hardpoint_id;
@@ -71,7 +72,7 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
/*
* Publish the command message to the bus
*/
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
_uavcan_pub_raw_cmd.broadcast(_cmd);
/*
* Start the periodic update timer after a command is set
@@ -81,10 +82,10 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
}
}
void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
void
UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
{
/*
* Broadcast command at MAX_RATE_HZ
*/
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
// Broadcast command at MAX_RATE_HZ
_uavcan_pub_raw_cmd.broadcast(_cmd);
}