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

rc_input: move to PX4 WQ

Browse Source
This commit is contained in:
Daniel Agar
2020-01-08 13:00:59 -05:00
committed by GitHub
parent ef9a930816
commit 5f1121c216
2 changed files with 42 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/rc_input/RCInput.cpp
+34 -126
View File
@@ -41,10 +41,10 @@ using namespace time_literals;
static bool bind_spektrum(int arg);
#endif /* SPEKTRUM_POWER */
work_s RCInput::_work = {};
constexpr char const *RCInput::RC_SCAN_STRING[];
RCInput::RCInput(bool run_as_task, char *device) :
RCInput::RCInput(const char *device) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
{
@@ -75,9 +75,7 @@ RCInput::~RCInput()
dsm_deinit();
#endif
if (_crsf_telemetry) {
delete (_crsf_telemetry);
}
delete _crsf_telemetry;
perf_free(_cycle_perf);
perf_free(_publish_interval_perf);
@@ -120,7 +118,6 @@ RCInput::init()
int
RCInput::task_spawn(int argc, char *argv[])
{
bool run_as_task = false;
bool error_flag = false;
int myoptind = 1;
@@ -128,12 +125,8 @@ RCInput::task_spawn(int argc, char *argv[])
const char *myoptarg = nullptr;
const char *device = RC_SERIAL_PORT;
while ((ch = px4_getopt(argc, argv, "td:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't':
run_as_task = true;
break;
case 'd':
device = myoptarg;
break;
@@ -153,71 +146,21 @@ RCInput::task_spawn(int argc, char *argv[])
return -1;
}
RCInput *instance = new RCInput(device);
if (!run_as_task) {
/* schedule a cycle to start things */
int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline_init, (void *)device, 0);
if (ret < 0) {
return ret;
}
// we need to wait, otherwise 'device' could go out of scope while still being accessed
wait_until_running();
_task_id = task_id_is_work_queue;
} else {
/* start the IO interface task */
const char *const args[] = { device, nullptr };
_task_id = px4_task_spawn_cmd("rc_input",
SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER,
1000,
(px4_main_t)&run_trampoline,
(char *const *)args);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(instance);
_task_id = task_id_is_work_queue;
instance->ScheduleOnInterval(_current_update_interval);
return PX4_OK;
}
void
RCInput::cycle_trampoline_init(void *arg)
{
RCInput *dev = new RCInput(false, (char *)arg);
if (!dev) {
PX4_ERR("alloc failed");
return;
}
int ret = dev->init();
if (ret != 0) {
PX4_ERR("init failed (%i)", ret);
delete dev;
return;
}
_object.store(dev);
dev->cycle();
}
void
RCInput::cycle_trampoline(void *arg)
{
RCInput *dev = static_cast<RCInput *>(arg);
dev->cycle();
}
void
RCInput::fill_rc_in(uint16_t raw_rc_count_local,
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
@@ -286,7 +229,7 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
#ifdef RC_SERIAL_PORT
void RCInput::set_rc_scan_state(RC_SCAN newState)
{
// PX4_WARN("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
_rc_scan_begin = 0;
_rc_scan_state = newState;
}
@@ -301,24 +244,23 @@ void RCInput::rc_io_invert(bool invert)
}
#endif
void
RCInput::run()
void RCInput::Run()
{
int ret = init();
if (ret != 0) {
PX4_ERR("init failed (%i)", ret);
if (should_exit()) {
exit_and_cleanup();
return;
}
cycle();
}
if (!_initialized) {
if (init() == PX4_OK) {
_initialized = true;
void
RCInput::cycle()
{
while (true) {
} else {
PX4_ERR("init failed");
exit_and_cleanup();
}
} else {
perf_begin(_cycle_perf);
@@ -403,17 +345,13 @@ RCInput::cycle()
int newBytes = 0;
if (_run_as_task) {
// TODO: needs work (poll _rcs_fd)
// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
// then update priority to SCHED_PRIORITY_FAST_DRIVER
// read all available data from the serial RC input UART
newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE);
// TODO: needs work (poll _rcs_fd)
// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
// then update priority to SCHED_PRIORITY_FAST_DRIVER
// read all available data from the serial RC input UART
} else {
// read all available data from the serial RC input UART
newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE);
}
// read all available data from the serial RC input UART
newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE);
switch (_rc_scan_state) {
case RC_SCAN_SBUS:
@@ -677,23 +615,6 @@ RCInput::cycle()
} else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1_s)) {
_rc_scan_locked = false;
}
if (_run_as_task) {
if (should_exit()) {
break;
}
} else {
if (should_exit()) {
exit_and_cleanup();
} else {
/* schedule next cycle */
work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, this, USEC2TICK(_current_update_interval));
}
break;
}
}
}
@@ -738,12 +659,6 @@ bool bind_spektrum(int arg)
}
#endif /* SPEKTRUM_POWER */
RCInput *RCInput::instantiate(int argc, char *argv[])
{
// No arguments to parse. We also know that we should run as task
return new RCInput(true, argv[0]);
}
int RCInput::custom_command(int argc, char *argv[])
{
#if defined(SPEKTRUM_POWER)
@@ -770,11 +685,9 @@ int RCInput::custom_command(int argc, char *argv[])
int RCInput::print_status()
{
PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue"));
PX4_INFO("Running");
if (!_run_as_task) {
PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval);
}
PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval);
if (_device[0] != '\0') {
PX4_INFO("Serial device: %s", _device);
@@ -816,15 +729,10 @@ This module does the RC input parsing and auto-selecting the method. Supported m
- ST24
- TBS Crossfire (CRSF)
### Implementation
By default the module runs on the work queue, to reduce RAM usage. It can also be run in its own thread,
specified via start flag -t, to reduce latency.
When running on the work queue, it schedules at a fixed frequency.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("rc_input", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)");
PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true);
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
#if defined(SPEKTRUM_POWER)
src/drivers/rc_input/RCInput.hpp
+8 -23
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2019 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
@@ -48,9 +48,9 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/workqueue.h>
#include <uORB/Subscription.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/adc_report.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command.h>
@@ -61,19 +61,16 @@
# include <systemlib/ppm_decode.h>
#endif
class RCInput : public ModuleBase<RCInput>
class RCInput : public ModuleBase<RCInput>, public px4::ScheduledWorkItem
{
public:
RCInput(bool run_as_task, char *device);
RCInput(const char *device);
virtual ~RCInput();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static RCInput *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@@ -81,12 +78,7 @@ public:
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/**
* run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle
*/
void cycle();
void Run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
@@ -114,14 +106,11 @@ private:
hrt_abstime _rc_scan_begin{0};
bool _initialized{false};
bool _rc_scan_locked{false};
bool _report_lock{true};
unsigned _current_update_interval{4000};
bool _run_as_task{false};
static struct work_s _work;
static constexpr unsigned _current_update_interval{4000}; // 250 Hz
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _adc_sub{ORB_ID(adc_report)};
@@ -146,10 +135,6 @@ private:
perf_counter_t _cycle_perf;
perf_counter_t _publish_interval_perf;
static void cycle_trampoline(void *arg);
static void cycle_trampoline_init(void *arg);
int start();
void fill_rc_in(uint16_t raw_rc_count_local,
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,