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

uavcan: move to work queue and use MixingOutput

Browse Source 
Main UAVCAN protocol handling and ESC updates run on the same thread/wq as
before. There are 2 WorkItems for separate scheduling of the 2, so that
ESC updates run in sync with actuator_control updates. UAVCAN is scheduled
at a fixed rate of 3ms (previously the poll timeout) and on each UAVCAN
bus event.
This leads to roughly the same behavior as before. CPU & RAM usage are
pretty much the same (tested on Pixhawk 4).

Testing done: Motors still work (with feedback), param changes and a
UAVCAN optical flow sensor.
This commit is contained in:
Beat Küng
2019-11-07 10:40:03 +01:00
parent b7a480b45b
commit 5dff065ec5
17 changed files with 354 additions and 908 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/actuators/esc.cpp
+10 -60
View File
@@ -56,8 +56,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
UavcanEscController::~UavcanEscController()
{
perf_free(_perfcnt_invalid_input);
perf_free(_perfcnt_scaling_error);
}
int
@@ -79,14 +77,14 @@ UavcanEscController::init()
}
void
UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
if (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) {
num_outputs = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
}
perf_count(_perfcnt_invalid_input);
return;
if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) {
num_outputs = esc_status_s::CONNECTED_ESC_MAX;
}
/*
@@ -106,34 +104,12 @@ UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
*/
uavcan::equipment::esc::RawCommand msg;
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = num_outputs;
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;
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
// trim negative values back to minimum
if (scaled < cmd_min) {
scaled = cmd_min;
perf_count(_perfcnt_scaling_error);
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
}
msg.cmd.push_back(static_cast<int>(scaled));
actuator_outputs.output[i] = scaled;
if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
msg.cmd.push_back(static_cast<unsigned>(0));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
actuator_outputs.output[i] = 0.0f;
msg.cmd.push_back(static_cast<int>(outputs[i]));
}
}
@@ -161,32 +137,6 @@ UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* Note that for a quadrotor it takes one CAN frame
*/
_uavcan_pub_raw_cmd.broadcast(msg);
// Publish actuator outputs
actuator_outputs.timestamp = hrt_absolute_time();
_actuator_outputs_pub.publish(actuator_outputs);
}
void
UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
}
void
UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
}
void
@@ -212,7 +162,7 @@ UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
_esc_status.esc_online_flags = check_escs_status();
_esc_status_pub.publish(_esc_status);
}
src/drivers/uavcan/actuators/esc.hpp
+8 -19
View File
@@ -52,28 +52,28 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/esc_status.h>
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
class UavcanEscController
{
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;
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
int init();
void update_outputs(float *outputs, unsigned num_outputs);
void arm_all_escs(bool arm);
void arm_single_esc(int num, bool arm);
void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
/**
* Sets the number of rotors
*/
void set_rotor_count(uint8_t count) { _rotor_count = count; }
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
private:
/**
@@ -100,15 +100,11 @@ private:
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)};
uint8_t _rotor_count = 0;
uint8_t _rotor_count{0};
/*
* libuavcan related things
@@ -122,12 +118,5 @@ private:
/*
* ESC states
*/
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")};
};