mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 18:06:39 +08:00
uavcan: move to work queue and use MixingOutput
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
@@ -225,7 +225,6 @@ class Graph(object):
|
||||
('listener', r'.*', None, r'^(id)$'),
|
||||
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
|
||||
|
||||
('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
|
||||
('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
|
||||
|
||||
@@ -64,6 +64,8 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10};
|
||||
|
||||
static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors
|
||||
|
||||
static constexpr wq_config_t uavcan{"uavcan", 2400, -13};
|
||||
|
||||
static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12};
|
||||
static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
|
||||
|
||||
|
||||
@@ -129,6 +129,8 @@ px4_add_module(
|
||||
px4_uavcan_dsdlc
|
||||
|
||||
mixer
|
||||
mixer_module
|
||||
output_limit
|
||||
version
|
||||
|
||||
git_uavcan
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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")};
|
||||
};
|
||||
|
||||
@@ -318,6 +318,8 @@ public:
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
BusEvent& updateEvent() { return update_event_; }
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -28,30 +28,17 @@ class CanDriver;
|
||||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
static const unsigned MaxPollWaiters = 8;
|
||||
|
||||
::file_operations file_ops_;
|
||||
::pollfd* pollset_[MaxPollWaiters];
|
||||
CanDriver& can_driver_;
|
||||
bool signal_;
|
||||
|
||||
static int openTrampoline(::file* filp);
|
||||
static int closeTrampoline(::file* filp);
|
||||
static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup);
|
||||
|
||||
int open(::file* filp);
|
||||
int close(::file* filp);
|
||||
int poll(::file* filp, ::pollfd* fds, bool setup);
|
||||
|
||||
int addPollWaiter(::pollfd* fds);
|
||||
int removePollWaiter(::pollfd* fds);
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
public:
|
||||
static const char* const DevName;
|
||||
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
|
||||
@@ -11,148 +11,49 @@
|
||||
namespace uavcan_kinetis
|
||||
{
|
||||
|
||||
|
||||
const unsigned BusEvent::MaxPollWaiters;
|
||||
const char* const BusEvent::DevName = "/dev/uavcan/busevent";
|
||||
|
||||
int BusEvent::openTrampoline(::file* filp)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->open(filp);
|
||||
}
|
||||
|
||||
int BusEvent::closeTrampoline(::file* filp)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->close(filp);
|
||||
}
|
||||
|
||||
int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->poll(filp, fds, setup);
|
||||
}
|
||||
|
||||
int BusEvent::open(::file* filp)
|
||||
{
|
||||
(void)filp;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BusEvent::close(::file* filp)
|
||||
{
|
||||
(void)filp;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup)
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
int ret = -1;
|
||||
|
||||
if (setup)
|
||||
{
|
||||
ret = addPollWaiter(fds);
|
||||
if (ret == 0)
|
||||
{
|
||||
/*
|
||||
* Two events can be reported via POLLIN:
|
||||
* - The RX queue is not empty. This event is level-triggered.
|
||||
* - Transmission complete. This event is edge-triggered.
|
||||
* FIXME Since TX event is edge-triggered, it can be lost between poll() calls.
|
||||
*/
|
||||
fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0);
|
||||
if (fds->revents != 0)
|
||||
{
|
||||
(void)sem_post(fds->sem);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ret = removePollWaiter(fds);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int BusEvent::addPollWaiter(::pollfd* fds)
|
||||
{
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
{
|
||||
if (pollset_[i] == UAVCAN_NULLPTR)
|
||||
{
|
||||
pollset_[i] = fds;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
int BusEvent::removePollWaiter(::pollfd* fds)
|
||||
{
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
{
|
||||
if (fds == pollset_[i])
|
||||
{
|
||||
pollset_[i] = UAVCAN_NULLPTR;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
: can_driver_(can_driver),
|
||||
signal_(false)
|
||||
{
|
||||
std::memset(&file_ops_, 0, sizeof(file_ops_));
|
||||
std::memset(pollset_, 0, sizeof(pollset_));
|
||||
file_ops_.open = &BusEvent::openTrampoline;
|
||||
file_ops_.close = &BusEvent::closeTrampoline;
|
||||
file_ops_.poll = &BusEvent::pollTrampoline;
|
||||
// TODO: move to init(), add proper error handling
|
||||
if (register_driver(DevName, &file_ops_, 0666, static_cast<void*>(this)) != 0)
|
||||
{
|
||||
std::abort();
|
||||
}
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
(void)unregister_driver(DevName);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
// TODO blocking wait
|
||||
const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration;
|
||||
while (clock::getMonotonic() < deadline)
|
||||
{
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
if (signal_)
|
||||
{
|
||||
signal_ = false;
|
||||
return true;
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
|
||||
int ret;
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
::usleep(1000);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void BusEvent::signalFromInterrupt()
|
||||
{
|
||||
signal_ = true; // HACK
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
if (sem_.semcount <= 0)
|
||||
{
|
||||
::pollfd* const fd = pollset_[i];
|
||||
if (fd != UAVCAN_NULLPTR)
|
||||
{
|
||||
fd->revents |= fd->events & POLLIN;
|
||||
if ((fd->revents != 0) && (fd->sem->semcount <= 0))
|
||||
{
|
||||
(void)sem_post(fd->sem);
|
||||
}
|
||||
}
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
if (signal_cb_)
|
||||
{
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -282,6 +282,8 @@ public:
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
BusEvent& updateEvent() { return update_event_; }
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -66,30 +66,17 @@ public:
|
||||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
static const unsigned MaxPollWaiters = 8;
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
::file_operations file_ops_;
|
||||
::pollfd* pollset_[MaxPollWaiters];
|
||||
CanDriver& can_driver_;
|
||||
bool signal_;
|
||||
|
||||
static int openTrampoline(::file* filp);
|
||||
static int closeTrampoline(::file* filp);
|
||||
static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup);
|
||||
|
||||
int open(::file* filp);
|
||||
int close(::file* filp);
|
||||
int poll(::file* filp, ::pollfd* fds, bool setup);
|
||||
|
||||
int addPollWaiter(::pollfd* fds);
|
||||
int removePollWaiter(::pollfd* fds);
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
|
||||
public:
|
||||
static const char* const DevName;
|
||||
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
|
||||
@@ -138,147 +138,49 @@ void Mutex::unlock()
|
||||
|
||||
#elif UAVCAN_STM32_NUTTX
|
||||
|
||||
const unsigned BusEvent::MaxPollWaiters;
|
||||
const char* const BusEvent::DevName = "/dev/uavcan/busevent";
|
||||
|
||||
int BusEvent::openTrampoline(::file* filp)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->open(filp);
|
||||
}
|
||||
|
||||
int BusEvent::closeTrampoline(::file* filp)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->close(filp);
|
||||
}
|
||||
|
||||
int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup)
|
||||
{
|
||||
return static_cast<BusEvent*>(filp->f_inode->i_private)->poll(filp, fds, setup);
|
||||
}
|
||||
|
||||
int BusEvent::open(::file* filp)
|
||||
{
|
||||
(void)filp;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BusEvent::close(::file* filp)
|
||||
{
|
||||
(void)filp;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup)
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
int ret = -1;
|
||||
|
||||
if (setup)
|
||||
{
|
||||
ret = addPollWaiter(fds);
|
||||
if (ret == 0)
|
||||
{
|
||||
/*
|
||||
* Two events can be reported via POLLIN:
|
||||
* - The RX queue is not empty. This event is level-triggered.
|
||||
* - Transmission complete. This event is edge-triggered.
|
||||
* FIXME Since TX event is edge-triggered, it can be lost between poll() calls.
|
||||
*/
|
||||
fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0);
|
||||
if (fds->revents != 0)
|
||||
{
|
||||
(void)sem_post(fds->sem);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ret = removePollWaiter(fds);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int BusEvent::addPollWaiter(::pollfd* fds)
|
||||
{
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
{
|
||||
if (pollset_[i] == UAVCAN_NULLPTR)
|
||||
{
|
||||
pollset_[i] = fds;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
int BusEvent::removePollWaiter(::pollfd* fds)
|
||||
{
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
{
|
||||
if (fds == pollset_[i])
|
||||
{
|
||||
pollset_[i] = UAVCAN_NULLPTR;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
: can_driver_(can_driver)
|
||||
, signal_(false)
|
||||
{
|
||||
std::memset(&file_ops_, 0, sizeof(file_ops_));
|
||||
std::memset(pollset_, 0, sizeof(pollset_));
|
||||
file_ops_.open = &BusEvent::openTrampoline;
|
||||
file_ops_.close = &BusEvent::closeTrampoline;
|
||||
file_ops_.poll = &BusEvent::pollTrampoline;
|
||||
// TODO: move to init(), add proper error handling
|
||||
if (register_driver(DevName, &file_ops_, 0666, static_cast<void*>(this)) != 0)
|
||||
{
|
||||
std::abort();
|
||||
}
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
(void)unregister_driver(DevName);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
// TODO blocking wait
|
||||
const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration;
|
||||
while (clock::getMonotonic() < deadline)
|
||||
{
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
if (signal_)
|
||||
{
|
||||
signal_ = false;
|
||||
return true;
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
|
||||
int ret;
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
::usleep(1000);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void BusEvent::signalFromInterrupt()
|
||||
{
|
||||
signal_ = true; // HACK
|
||||
for (unsigned i = 0; i < MaxPollWaiters; i++)
|
||||
if (sem_.semcount <= 0)
|
||||
{
|
||||
::pollfd* const fd = pollset_[i];
|
||||
if (fd != UAVCAN_NULLPTR)
|
||||
{
|
||||
fd->revents |= fd->events & POLLIN;
|
||||
if ((fd->revents != 0) && (fd->sem->semcount <= 0))
|
||||
{
|
||||
(void)sem_post(fd->sem);
|
||||
}
|
||||
}
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
if (signal_cb_)
|
||||
{
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+146
-392
File diff suppressed because it is too large
Load Diff
@@ -43,6 +43,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include "uavcan_driver.hpp"
|
||||
#include "uavcan_servers.hpp"
|
||||
@@ -60,31 +62,56 @@
|
||||
#include <uavcan/protocol/RestartNode.hpp>
|
||||
|
||||
#include <lib/drivers/device/device.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
|
||||
class UavcanNode;
|
||||
|
||||
// we add 1 to allow for busevent
|
||||
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+1)
|
||||
/**
|
||||
* UAVCAN mixing class.
|
||||
* It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
|
||||
* (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
|
||||
* a fixed rate or upon bus updates).
|
||||
* Both work items are expected to run on the same work queue.
|
||||
*/
|
||||
class UavcanMixingInterface : public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
|
||||
: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
|
||||
_node_mutex(node_mutex),
|
||||
_esc_controller(esc_controller) {}
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
void mixerChanged() override;
|
||||
|
||||
MixingOutput &mixingOutput() { return _mixing_output; }
|
||||
|
||||
protected:
|
||||
void Run() override;
|
||||
private:
|
||||
friend class UavcanNode;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
UavcanEscController &_esc_controller;
|
||||
MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
};
|
||||
|
||||
/**
|
||||
* A UAVCAN node.
|
||||
*/
|
||||
class UavcanNode : public cdev::CDev
|
||||
class UavcanNode : public cdev::CDev, public px4::ScheduledWorkItem, public ModuleParams
|
||||
{
|
||||
static constexpr unsigned MaxBitRatePerSec = 1000000;
|
||||
static constexpr unsigned bitPerFrame = 148;
|
||||
static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
|
||||
static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
|
||||
|
||||
static constexpr unsigned PollTimeoutMs = 3;
|
||||
static constexpr unsigned ScheduleIntervalMs = 3;
|
||||
|
||||
|
||||
/*
|
||||
@@ -92,19 +119,18 @@ class UavcanNode : public cdev::CDev
|
||||
* At 1Mbit there is approximately one CAN frame every 145 uS.
|
||||
* The number of buffers sets how long you can go without calling
|
||||
* node_spin_xxxx. Since our task is the only one running and the
|
||||
* driver will light the fd when there is a CAN frame we can nun with
|
||||
* driver will light the callback when there is a CAN frame we can nun with
|
||||
* a minimum number of buffers to conserver memory. Each buffer is
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
|
||||
* of ~1 mS
|
||||
* 1000000/200
|
||||
*/
|
||||
|
||||
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
|
||||
static constexpr unsigned StackSize = 2400;
|
||||
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs; // At
|
||||
|
||||
public:
|
||||
typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
enum eServerAction {None, Start, Stop, CheckFW, Busy};
|
||||
enum eServerAction : int {None, Start, Stop, CheckFW, Busy};
|
||||
|
||||
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
|
||||
|
||||
@@ -116,15 +142,8 @@ public:
|
||||
|
||||
uavcan::Node<> &get_node() { return _node; }
|
||||
|
||||
// TODO: move the actuator mixing stuff into the ESC controller class
|
||||
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
|
||||
|
||||
void subscribe();
|
||||
|
||||
int teardown();
|
||||
|
||||
int arm_actuators(bool arm);
|
||||
|
||||
void print_info();
|
||||
|
||||
void shrink();
|
||||
@@ -141,13 +160,14 @@ public:
|
||||
int get_param(int remote_node_id, const char *name);
|
||||
int reset_node(int remote_node_id);
|
||||
|
||||
static void busevent_signal_trampoline();
|
||||
|
||||
protected:
|
||||
void Run() override;
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
|
||||
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
|
||||
|
||||
int start_fw_server();
|
||||
int stop_fw_server();
|
||||
@@ -160,16 +180,14 @@ private:
|
||||
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
|
||||
void free_setget_response(void) { _setget_response = nullptr; }
|
||||
|
||||
int _task{-1}; ///< handle to the OS task
|
||||
bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
|
||||
volatile eServerAction _fw_server_action{None};
|
||||
void enable_idle_throttle_when_armed(bool value);
|
||||
|
||||
px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
|
||||
px4::atomic<int> _fw_server_action{None};
|
||||
int _fw_server_status{-1};
|
||||
|
||||
bool _is_armed{false}; ///< the arming status of the actuators on the bus
|
||||
|
||||
test_motor_s _test_motor{};
|
||||
bool _test_in_progress{false};
|
||||
|
||||
unsigned _output_count{0}; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
@@ -177,9 +195,10 @@ private:
|
||||
uavcan_node::Allocator _pool_allocator;
|
||||
|
||||
uavcan::Node<> _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex{};
|
||||
pthread_mutex_t _node_mutex;
|
||||
px4_sem_t _server_command_sem;
|
||||
UavcanEscController _esc_controller;
|
||||
UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller};
|
||||
UavcanHardpointController _hardpoint_controller;
|
||||
uavcan::GlobalTimeSyncMaster _time_sync_master;
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
@@ -187,31 +206,15 @@ private:
|
||||
|
||||
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
|
||||
|
||||
MixerGroup *_mixers{nullptr};
|
||||
ITxQueueInjector *_tx_injector{nullptr};
|
||||
|
||||
uint32_t _groups_required{0};
|
||||
uint32_t _groups_subscribed{0};
|
||||
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
|
||||
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
|
||||
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] {};
|
||||
unsigned _poll_fds_num{0};
|
||||
int32_t _idle_throttle_when_armed{0};
|
||||
bool _idle_throttle_when_armed{false};
|
||||
int32_t _idle_throttle_when_armed_param{0};
|
||||
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _test_motor_sub{ORB_ID(test_motor)};
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
perf_counter_t _control_latency_perf;
|
||||
|
||||
Mixer::Airmode _airmode{Mixer::Airmode::disabled};
|
||||
float _thr_mdl_factor{0.0f};
|
||||
|
||||
// index into _poll_fds for each _control_subs handle
|
||||
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
|
||||
|
||||
void handle_time_sync(const uavcan::TimerEvent &);
|
||||
|
||||
|
||||
@@ -57,6 +57,8 @@
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
*
|
||||
* Implements basic functionality of UAVCAN esc.
|
||||
@@ -99,6 +101,10 @@ UavcanEsc::UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &syste
|
||||
std::abort();
|
||||
}
|
||||
|
||||
px4_sem_init(&_sem, 0, 0);
|
||||
/* semaphore use case is a signal */
|
||||
px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
|
||||
|
||||
}
|
||||
|
||||
UavcanEsc::~UavcanEsc()
|
||||
@@ -123,7 +129,7 @@ UavcanEsc::~UavcanEsc()
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
|
||||
px4_sem_destroy(&_sem);
|
||||
}
|
||||
|
||||
int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
@@ -163,7 +169,7 @@ int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
}
|
||||
|
||||
|
||||
const int node_init_res = _instance->init(node_id);
|
||||
const int node_init_res = _instance->init(node_id, can.driver.updateEvent());
|
||||
|
||||
if (node_init_res < 0) {
|
||||
delete _instance;
|
||||
@@ -255,7 +261,7 @@ void UavcanEsc::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<Uavc
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanEsc::init(uavcan::NodeID node_id)
|
||||
int UavcanEsc::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
@@ -279,6 +285,8 @@ int UavcanEsc::init(uavcan::NodeID node_id)
|
||||
return ret;
|
||||
}
|
||||
|
||||
bus_events.registerSignalCallback(UavcanEsc::busevent_signal_trampoline);
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
@@ -306,26 +314,28 @@ void UavcanEsc::node_spin_once()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
add a fd to the list of polled events. This assumes you want
|
||||
POLLIN for now.
|
||||
*/
|
||||
int UavcanEsc::add_poll_fd(int fd)
|
||||
static void signal_callback(void *arg)
|
||||
{
|
||||
int ret = _poll_fds_num;
|
||||
/* Note: we are in IRQ context here */
|
||||
px4_sem_t *sem = (px4_sem_t *)arg;
|
||||
int semaphore_value;
|
||||
|
||||
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
|
||||
errx(1, "uavcan: too many poll fds, exiting");
|
||||
if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
_poll_fds[_poll_fds_num] = ::pollfd();
|
||||
_poll_fds[_poll_fds_num].fd = fd;
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num += 1;
|
||||
return ret;
|
||||
px4_sem_post(sem);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
UavcanEsc::busevent_signal_trampoline()
|
||||
{
|
||||
if (_instance) {
|
||||
signal_callback(&_instance->_sem);
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanEsc::run()
|
||||
{
|
||||
|
||||
@@ -338,54 +348,28 @@ int UavcanEsc::run()
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
const unsigned PollTimeoutMs = 50;
|
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||
|
||||
if (busevent_fd < 0) {
|
||||
PX4_WARN("Failed to open %s", uavcan_stm32::BusEvent::DevName);
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
|
||||
* but for now we use adjustUtc with a correction of 0
|
||||
*/
|
||||
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
/*
|
||||
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
|
||||
* Please note that with such multiplexing it is no longer possible to rely only on
|
||||
* the value returned from poll() to detect whether actuator control has timed out or not.
|
||||
* Instead, all ORB events need to be checked individually (see below).
|
||||
*/
|
||||
add_poll_fd(busevent_fd);
|
||||
hrt_call timer_call{};
|
||||
hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem);
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// Mutex is unlocked while the thread is blocked on IO multiplexing
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
|
||||
|
||||
while (px4_sem_wait(&_sem) != 0);
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
// Do Something
|
||||
|
||||
// this would be bad...
|
||||
if (poll_ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
continue;
|
||||
|
||||
} else {
|
||||
// Do Something
|
||||
}
|
||||
}
|
||||
|
||||
hrt_cancel(&timer_call);
|
||||
teardown();
|
||||
PX4_WARN("exiting.");
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -115,12 +115,13 @@ public:
|
||||
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
|
||||
static void busevent_signal_trampoline();
|
||||
|
||||
|
||||
px4_sem_t _sem; ///< semaphore for scheduling the task
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
|
||||
@@ -128,9 +129,6 @@ private:
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
|
||||
unsigned _poll_fds_num = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEsc *,
|
||||
void (UavcanEsc::*)(const uavcan::ReceivedDataStructure<UavcanEsc::BeginFirmwareUpdate::Request> &,
|
||||
uavcan::ServiceResponseDataStructure<UavcanEsc::BeginFirmwareUpdate::Response> &)>
|
||||
|
||||
@@ -64,6 +64,8 @@ __END_DECLS
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
*
|
||||
@@ -124,6 +126,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
||||
std::abort();
|
||||
}
|
||||
|
||||
px4_sem_init(&_sem, 0, 0);
|
||||
/* semaphore use case is a signal */
|
||||
px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
UavcanNode::~UavcanNode()
|
||||
@@ -148,7 +153,7 @@ UavcanNode::~UavcanNode()
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
|
||||
px4_sem_destroy(&_sem);
|
||||
}
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
@@ -189,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
|
||||
|
||||
resources("Before _instance->init:");
|
||||
const int node_init_res = _instance->init(node_id);
|
||||
const int node_init_res = _instance->init(node_id, can.driver.updateEvent());
|
||||
resources("After _instance->init:");
|
||||
|
||||
if (node_init_res < 0) {
|
||||
@@ -282,7 +287,7 @@ void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<Uav
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id)
|
||||
int UavcanNode::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
@@ -306,6 +311,8 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
||||
return ret;
|
||||
}
|
||||
|
||||
bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
@@ -333,26 +340,30 @@ void UavcanNode::node_spin_once()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
add a fd to the list of polled events. This assumes you want
|
||||
POLLIN for now.
|
||||
*/
|
||||
int UavcanNode::add_poll_fd(int fd)
|
||||
static void signal_callback(void *arg)
|
||||
{
|
||||
int ret = _poll_fds_num;
|
||||
/* Note: we are in IRQ context here */
|
||||
px4_sem_t *sem = (px4_sem_t *)arg;
|
||||
int semaphore_value;
|
||||
|
||||
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
|
||||
errx(1, "uavcan: too many poll fds, exiting");
|
||||
if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
_poll_fds[_poll_fds_num] = ::pollfd();
|
||||
_poll_fds[_poll_fds_num].fd = fd;
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num += 1;
|
||||
return ret;
|
||||
px4_sem_post(sem);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
UavcanNode::busevent_signal_trampoline()
|
||||
{
|
||||
if (_instance) {
|
||||
signal_callback(&_instance->_sem);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int UavcanNode::run()
|
||||
{
|
||||
|
||||
@@ -381,52 +392,26 @@ int UavcanNode::run()
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
const unsigned PollTimeoutMs = 50;
|
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||
|
||||
if (busevent_fd < 0) {
|
||||
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
|
||||
* but for now we use adjustUtc with a correction of 0
|
||||
*/
|
||||
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
/*
|
||||
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
|
||||
* Please note that with such multiplexing it is no longer possible to rely only on
|
||||
* the value returned from poll() to detect whether actuator control has timed out or not.
|
||||
* Instead, all ORB events need to be checked individually (see below).
|
||||
*/
|
||||
add_poll_fd(busevent_fd);
|
||||
|
||||
uint32_t start_tick = clock_systimer();
|
||||
|
||||
hrt_call timer_call{};
|
||||
hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem);
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// Mutex is unlocked while the thread is blocked on IO multiplexing
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
|
||||
|
||||
while (px4_sem_wait(&_sem) != 0);
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
|
||||
// this would be bad...
|
||||
if (poll_ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
continue;
|
||||
// Do Something
|
||||
|
||||
} else {
|
||||
// Do Something
|
||||
}
|
||||
|
||||
if (clock_systimer() - start_tick > TICK_PER_SEC) {
|
||||
start_tick = clock_systimer();
|
||||
@@ -455,8 +440,9 @@ int UavcanNode::run()
|
||||
|
||||
}
|
||||
|
||||
hrt_cancel(&timer_call);
|
||||
teardown();
|
||||
warnx("exiting.");
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -118,12 +118,13 @@ public:
|
||||
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
|
||||
static void busevent_signal_trampoline();
|
||||
|
||||
|
||||
px4_sem_t _sem; ///< semaphore for scheduling the task
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
|
||||
@@ -132,9 +133,6 @@ private:
|
||||
pthread_mutex_t _node_mutex;
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
|
||||
unsigned _poll_fds_num = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
void (UavcanNode::*)(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &)>
|
||||
|
||||
Reference in New Issue
Block a user