diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 43000cd9da..6b83f77513 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -215,11 +215,6 @@ else # tone_alarm start - if param compare SYS_FMU_TASK 1 - then - set FMU_ARGS "-t" - fi - # # Set parameters and env variables for selected AUTOSTART. # diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index cad97f050a..651cfe1cd6 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -229,7 +229,6 @@ class Graph(object): ('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('pwm_out_sim', 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\]$'), - ('fmu', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'), ] special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d)) diff --git a/boards/auav/x21/init/rc.board_defaults b/boards/auav/x21/init/rc.board_defaults index 617e498f09..a247fc961b 100644 --- a/boards/auav/x21/init/rc.board_defaults +++ b/boards/auav/x21/init/rc.board_defaults @@ -6,5 +6,5 @@ if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/boards/px4/fmu-v2/init/rc.board_defaults b/boards/px4/fmu-v2/init/rc.board_defaults index 702bbd1fc9..54cd09248d 100644 --- a/boards/px4/fmu-v2/init/rc.board_defaults +++ b/boards/px4/fmu-v2/init/rc.board_defaults @@ -24,5 +24,5 @@ unset BL_FILE if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/boards/px4/fmu-v3/init/rc.board_defaults b/boards/px4/fmu-v3/init/rc.board_defaults index 01238b0354..4749b503e8 100644 --- a/boards/px4/fmu-v3/init/rc.board_defaults +++ b/boards/px4/fmu-v3/init/rc.board_defaults @@ -6,5 +6,5 @@ if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 61e7e0435e..37e9b08d89 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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 @@ -47,17 +47,22 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -65,7 +70,7 @@ #include #include -#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ +using namespace time_literals; static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */ @@ -104,7 +109,7 @@ enum PortMode { #define PX4FMU_DEVICE_PATH "/dev/px4fmu" -class PX4FMU : public cdev::CDev, public ModuleBase +class PX4FMU : public cdev::CDev, public ModuleBase, public px4::ScheduledWorkItem { public: enum Mode { @@ -126,7 +131,7 @@ public: MODE_5CAP, MODE_6CAP, }; - PX4FMU(bool run_as_task); + PX4FMU(); virtual ~PX4FMU(); /** @see ModuleBase */ @@ -141,13 +146,10 @@ public: /** @see ModuleBase */ 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; @@ -186,70 +188,77 @@ private: hrt_abstime _last_safety_check = 0; hrt_abstime _time_last_mix = 0; - static constexpr unsigned _MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; + static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; - Mode _mode; - unsigned _pwm_default_rate; - unsigned _pwm_alt_rate; - uint32_t _pwm_alt_rate_channels; - unsigned _current_update_rate; - bool _run_as_task; - static struct work_s _work; + Mode _mode{MODE_NONE}; - int _armed_sub; - int _param_sub; - int _safety_sub; + unsigned _pwm_default_rate{50}; + unsigned _pwm_alt_rate{50}; + uint32_t _pwm_alt_rate_channels{0}; - orb_advert_t _outputs_pub; - unsigned _num_outputs; - int _class_instance; + unsigned _current_update_rate{0}; - bool _throttle_armed; - bool _pwm_on; - uint32_t _pwm_mask; - bool _pwm_initialized; - bool _test_mode; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + uORB::Subscription _safety_sub{ORB_ID(safety)}; - MixerGroup *_mixers; + uORB::Publication _to_safety{ORB_ID(safety)}; + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT}; + uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT}; ///< mixer status flags - uint32_t _groups_required; - uint32_t _groups_subscribed; - int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {-1, -1, -1, -1}; + unsigned _num_outputs{0}; + int _class_instance{-1}; + + bool _throttle_armed{false}; + bool _pwm_on{false}; + uint32_t _pwm_mask{0}; + bool _pwm_initialized{false}; + bool _test_mode{false}; + + MixerGroup *_mixers{nullptr}; + + uint32_t _groups_required{0}; + uint32_t _groups_subscribed{0}; + + bool _wq_hpdefault{true}; + + uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] { + {this, ORB_ID(actuator_controls_0)}, + {this, ORB_ID(actuator_controls_1)}, + {this, ORB_ID(actuator_controls_2)}, + {this, ORB_ID(actuator_controls_3)} + }; actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - unsigned _poll_fds_num; static pwm_limit_t _pwm_limit; static actuator_armed_s _armed; - uint16_t _failsafe_pwm[_MAX_ACTUATORS] {}; - uint16_t _disarmed_pwm[_MAX_ACTUATORS] {}; - uint16_t _min_pwm[_MAX_ACTUATORS] {}; - uint16_t _max_pwm[_MAX_ACTUATORS] {}; - uint16_t _reverse_pwm_mask; - unsigned _num_failsafe_set; - unsigned _num_disarmed_set; - bool _safety_off; ///< State of the safety button from the subscribed safety topic - bool _safety_btn_off; ///< State of the safety button read from the HW button - bool _safety_disabled; - orb_advert_t _to_safety; - orb_advert_t _to_mixer_status; ///< mixer status flags - float _mot_t_max; ///< maximum rise time for motor (slew rate limiting) - float _thr_mdl_fac; ///< thrust to pwm modelling factor - Mixer::Airmode _airmode; ///< multicopter air-mode - MotorOrdering _motor_ordering; + uint16_t _failsafe_pwm[MAX_ACTUATORS] {}; + uint16_t _disarmed_pwm[MAX_ACTUATORS] {}; + uint16_t _min_pwm[MAX_ACTUATORS] {}; + uint16_t _max_pwm[MAX_ACTUATORS] {}; + uint16_t _reverse_pwm_mask{0}; + unsigned _num_failsafe_set{0}; + unsigned _num_disarmed_set{0}; - perf_counter_t _perf_control_latency; + bool _safety_off{false}; ///< State of the safety button from the subscribed safety topic + bool _safety_btn_off{false}; ///< State of the safety button read from the HW button + bool _safety_disabled{false}; + + float _mot_t_max{0.0f}; ///< maximum rise time for motor (slew rate limiting) + float _thr_mdl_fac{0.0f}; ///< thrust to pwm modelling factor + Mixer::Airmode _airmode{Mixer::Airmode::disabled}; ///< multicopter air-mode + MotorOrdering _motor_ordering{MotorOrdering::PX4}; + + perf_counter_t _cycle_perf; + perf_counter_t _cycle_interval_perf; + perf_counter_t _control_latency_perf; static bool arm_nothrottle() { return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode); } - static void cycle_trampoline(void *arg); - int start(); - static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, @@ -279,67 +288,24 @@ private: * Reorder PWM outputs according to _motor_ordering * @param values PWM values to reorder */ - inline void reorder_outputs(uint16_t values[_MAX_ACTUATORS]); + inline void reorder_outputs(uint16_t values[MAX_ACTUATORS]); }; pwm_limit_t PX4FMU::_pwm_limit; actuator_armed_s PX4FMU::_armed = {}; -work_s PX4FMU::_work = {}; -PX4FMU::PX4FMU(bool run_as_task) : +PX4FMU::PX4FMU() : CDev(PX4FMU_DEVICE_PATH), - _mode(MODE_NONE), - _pwm_default_rate(50), - _pwm_alt_rate(50), - _pwm_alt_rate_channels(0), - _current_update_rate(0), - _run_as_task(run_as_task), - _armed_sub(-1), - _param_sub(-1), - _safety_sub(-1), - _outputs_pub(nullptr), - _num_outputs(0), - _class_instance(0), - _throttle_armed(false), - _pwm_on(false), - _pwm_mask(0), - _pwm_initialized(false), - _test_mode(false), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _poll_fds_num(0), - _reverse_pwm_mask(0), - _num_failsafe_set(0), - _num_disarmed_set(0), - _safety_off(false), - _safety_btn_off(false), - _safety_disabled(false), - _to_safety(nullptr), - _to_mixer_status(nullptr), - _mot_t_max(0.0f), - _thr_mdl_fac(0.0f), - _airmode(Mixer::Airmode::disabled), - _motor_ordering(MotorOrdering::PX4), - _perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency")) + ScheduledWorkItem(px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")), + _cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval")), + _control_latency_perf(perf_alloc(PC_ELAPSED, "px4fmu: control latency")) { - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; _max_pwm[i] = PWM_DEFAULT_MAX; } - _control_topics[0] = ORB_ID(actuator_controls_0); - _control_topics[1] = ORB_ID(actuator_controls_1); - _control_topics[2] = ORB_ID(actuator_controls_2); - _control_topics[3] = ORB_ID(actuator_controls_3); - - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) { - _control_subs[i] = -1; - } - - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); - // Safely initialize armed flags. _armed.armed = false; _armed.prearmed = false; @@ -357,38 +323,22 @@ PX4FMU::PX4FMU(bool run_as_task) : PX4FMU::~PX4FMU() { - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_unsubscribe(_control_subs[i]); - } - } - - orb_unsubscribe(_armed_sub); - orb_unsubscribe(_param_sub); - orb_unsubscribe(_safety_sub); - - orb_unadvertise(_outputs_pub); - orb_unadvertise(_to_safety); - orb_unadvertise(_to_mixer_status); - /* make sure servos are off */ up_pwm_servo_deinit(); - /* note - someone else is responsible for restoring the GPIO config */ - /* clean up the alternate device node */ unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - perf_free(_perf_control_latency); + perf_free(_cycle_perf); + perf_free(_cycle_interval_perf); + perf_free(_control_latency_perf); } int PX4FMU::init() { - int ret; - /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { return ret; @@ -415,16 +365,14 @@ PX4FMU::init() /* force a reset of the update rate */ _current_update_rate = 0; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - _safety_sub = orb_subscribe(ORB_ID(safety)); - /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); // Getting initial parameter values update_params(); + ScheduleOnInterval(100_ms); // run at 10 Hz until mixers loaded + return 0; } @@ -752,7 +700,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate for (unsigned pass = 0; pass < 2; pass++) { - /* We should note that group is iterated over from 0 to _MAX_ACTUATORS. + /* We should note that group is iterated over from 0 to MAX_ACTUATORS. * This allows for the ideal worlds situation: 1 channel per group * configuration. * @@ -766,7 +714,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate * rate and mode. (See rates above.) */ - for (unsigned group = 0; group < _MAX_ACTUATORS; group++) { + for (unsigned group = 0; group < MAX_ACTUATORS; group++) { // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); @@ -808,6 +756,8 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate _pwm_default_rate = default_rate; _pwm_alt_rate = alt_rate; + _current_update_rate = 0; // force update + return OK; } @@ -820,29 +770,71 @@ PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz) void PX4FMU::subscribe() { - /* subscribe/unsubscribe to required actuator control groups */ - uint32_t sub_groups = _groups_required & ~_groups_subscribed; - uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; + // must be locked to potentially change WorkQueue + lock(); - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (sub_groups & (1 << i)) { - PX4_DEBUG("subscribe to actuator_controls_%d", i); - _control_subs[i] = orb_subscribe(_control_topics[i]); - } + // first clear everything + ScheduleClear(); - if (unsub_groups & (1 << i)) { - PX4_DEBUG("unsubscribe from actuator_controls_%d", i); - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } + for (auto &control_sub : _control_subs) { + control_sub.unregister_callback(); + } - if (_control_subs[i] >= 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num++; + // if subscribed to control group 0 or 1 then move to the rate_ctrl WQ + const bool sub_group_0 = (_groups_required & (1 << 0)); + const bool sub_group_1 = (_groups_required & (1 << 1)); + + if (_wq_hpdefault && (sub_group_0 || sub_group_1)) { + if (WorkItem::Init(px4::wq_configurations::rate_ctrl)) { + // let the new WQ handle the subscribe update + _wq_hpdefault = false; + ScheduleNow(); + unlock(); + return; } } + + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + int max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + + // oneshot + if ((_pwm_default_rate == 0) || (_pwm_alt_rate == 0)) { + max_rate = 2000; + } + + // max interval 0.5 - 100 ms (10 - 2000Hz) + const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000); + _current_update_rate = max_rate; + _groups_subscribed = _groups_required; + + // subscribe to all required actuator control groups with max interval set + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_required & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + + if (!_control_subs[i].register_callback()) { + PX4_ERR("actuator_controls_%d register callback failed!", i); + } + + // limit subscription interval + _control_subs[i].set_interval_us(update_interval_in_us); + } + } + + // if nothing required keep periodic schedule for safety button + if (_groups_required == 0) { + ScheduleOnInterval(100_ms); + } + + PX4_DEBUG("_groups_required 0x%08x", _groups_required); + PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed); + + unlock(); } void @@ -863,7 +855,7 @@ PX4FMU::update_pwm_rev_mask() return; } - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { char pname[16]; /* fill the channel reverse mask from parameters */ @@ -885,7 +877,7 @@ PX4FMU::update_pwm_trims() if (_mixers != nullptr) { - int16_t values[_MAX_ACTUATORS] = {}; + int16_t values[MAX_ACTUATORS] = {}; const char *pname_format; @@ -900,7 +892,7 @@ PX4FMU::update_pwm_trims() return; } - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { char pname[16]; /* fill the struct from parameters */ @@ -916,7 +908,7 @@ PX4FMU::update_pwm_trims() } /* copy the trim values to the mixer offsets */ - unsigned n_out = _mixers->set_trims(values, _MAX_ACTUATORS); + unsigned n_out = _mixers->set_trims(values, MAX_ACTUATORS); PX4_DEBUG("set %d trims", n_out); } } @@ -924,96 +916,25 @@ PX4FMU::update_pwm_trims() int PX4FMU::task_spawn(int argc, char *argv[]) { - bool run_as_task = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "t", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 't': - run_as_task = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return -1; - } - - - if (!run_as_task) { - - /* schedule a cycle to start things */ - int ret = work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, nullptr, 0); - - if (ret < 0) { - return ret; - } + PX4FMU *instance = new PX4FMU(); + if (instance) { + _object.store(instance); _task_id = task_id_is_work_queue; + if (instance->init() == PX4_OK) { + return PX4_OK; + } + } else { - - /* start the IO interface task */ - - _task_id = px4_task_spawn_cmd("fmu", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1340, - (px4_main_t)&run_trampoline, - nullptr); - - if (_task_id < 0) { - _task_id = -1; - return -errno; - } + PX4_ERR("alloc failed"); } - // wait until task is up & running (the mode_* commands depend on it) - if (wait_until_running() < 0) { - _task_id = -1; - return -1; - } + delete instance; + _object.store(nullptr); + _task_id = -1; - return PX4_OK; -} - -void -PX4FMU::cycle_trampoline(void *arg) -{ - PX4FMU *dev = reinterpret_cast(arg); - - // check if the trampoline is called for the first time - if (!dev) { - dev = new PX4FMU(false); - - if (!dev) { - PX4_ERR("alloc failed"); - return; - } - - if (dev->init() != 0) { - PX4_ERR("init failed"); - delete dev; - return; - } - - _object.store(dev); - } - - dev->cycle(); + return PX4_ERROR; } void @@ -1044,320 +965,213 @@ PX4FMU::update_pwm_out_state(bool on) } void -PX4FMU::run() +PX4FMU::Run() { - if (init() != 0) { - PX4_ERR("init failed"); + if (should_exit()) { + ScheduleClear(); + + for (auto &control_sub : _control_subs) { + control_sub.unregister_callback(); + } + exit_and_cleanup(); return; } - cycle(); -} + perf_begin(_cycle_perf); + perf_count(_cycle_interval_perf); -void -PX4FMU::cycle() -{ - while (true) { + // check arming state + if (_armed_sub.update(&_armed)) { + /* Update the armed status and check that we're not locked down. + * We also need to arm throttle for the ESC calibration. */ + _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode); + } - if (_groups_subscribed != _groups_required) { - subscribe(); - _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; - } + unsigned n_updates = 0; - int poll_timeout = 10; - - if (!_run_as_task) { - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; + if (_mixers != nullptr) { + /* get controls for required topics */ + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_subscribed & (1 << i)) { + if (_control_subs[i].copy(&_controls[i])) { + n_updates++; } - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { - update_rate_in_ms = 100; - } + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { - PX4_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_set_interval(_control_subs[i], update_rate_in_ms); - } - } + /* except thrust to maximum. */ + _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; - } - - /* check if anything updated */ - poll_timeout = 0; - } - - /* wait for an update */ - unsigned n_updates = 0; - int ret = px4_poll(_poll_fds, _poll_fds_num, poll_timeout); - - /* this would be bad... */ - if (ret < 0) { - PX4_DEBUG("poll error %d", errno); - - } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ - // PX4_WARN("no PWM: failsafe"); - - } else { - if (_mixers != nullptr) { - /* get controls for required topics */ - unsigned poll_id = 0; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - - if (_poll_fds[poll_id].revents & POLLIN) { - if (i == 0) { - n_updates++; - } - - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - } - - poll_id++; - } - - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { - - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); - - /* except thrust to maximum. */ - _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - - /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; - } - } - } - } // poll_fds - - /* run the mixers on every cycle */ - { - if (_mixers != nullptr) { - - if (_mot_t_max > FLT_EPSILON) { - hrt_abstime now = hrt_absolute_time(); - float dt = (now - _time_last_mix) / 1e6f; - _time_last_mix = now; - - if (dt < 0.0001f) { - dt = 0.0001f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - // maximum value the outputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator outputs are in the range [-1,1] - const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; - _mixers->set_max_delta_out_once(delta_out_max); - } - - _mixers->set_thrust_factor(_thr_mdl_fac); - _mixers->set_airmode(_airmode); - - /* do mixing */ - float outputs[_MAX_ACTUATORS]; - const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs); - - /* the PWM limit call takes care of out of band errors, NaN and constrains */ - uint16_t pwm_limited[_MAX_ACTUATORS]; - - pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); - - /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ - if (_armed.force_failsafe) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - pwm_limited[i] = _failsafe_pwm[i]; - } - } - - /* overwrite outputs in case of lockdown or parachute triggering with disarmed PWM values */ - if (_armed.lockdown || _armed.manual_lockdown) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - pwm_limited[i] = _disarmed_pwm[i]; - } - } - - /* apply _motor_ordering */ - reorder_outputs(pwm_limited); - - /* output to the servos */ - if (_pwm_initialized && !_test_mode) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - } - - /* Trigger all timer's channels in Oneshot mode to fire - * the oneshots with updated values. - */ - if (n_updates > 0 && !_test_mode) { - up_pwm_update(); - } - - actuator_outputs_s actuator_outputs = {}; - actuator_outputs.timestamp = hrt_absolute_time(); - actuator_outputs.noutputs = mixed_num_outputs; - - // zero unused outputs - for (size_t i = 0; i < mixed_num_outputs; ++i) { - actuator_outputs.output[i] = pwm_limited[i]; - } - - orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &actuator_outputs, &_class_instance, ORB_PRIO_DEFAULT); - - /* publish mixer status */ - MultirotorMixer::saturation_status saturation_status; - saturation_status.value = _mixers->get_saturation_status(); - - if (saturation_status.flags.valid) { - multirotor_motor_limits_s motor_limits; - motor_limits.timestamp = hrt_absolute_time(); - motor_limits.saturation_status = saturation_status.value; - - orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance, ORB_PRIO_DEFAULT); - } - - // use first valid timestamp_sample for latency tracking - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - const bool required = _groups_required & (1 << i); - const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; - - if (required && (timestamp_sample > 0)) { - perf_set_elapsed(_perf_control_latency, actuator_outputs.timestamp - timestamp_sample); - break; - } + /* Switch off the PWM limit ramp for the calibration. */ + _pwm_limit.state = PWM_LIMIT_STATE_ON; } } } - _cycle_timestamp = hrt_absolute_time(); + if (_mot_t_max > FLT_EPSILON) { + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _time_last_mix) / 1e6f, 0.0001f, 0.02f); + _time_last_mix = now; + + // maximum value the outputs of the multirotor mixer are allowed to change in this cycle + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; + _mixers->set_max_delta_out_once(delta_out_max); + } + + if (_thr_mdl_fac > FLT_EPSILON) { + _mixers->set_thrust_factor(_thr_mdl_fac); + } + + /* do mixing */ + float outputs[MAX_ACTUATORS] {}; + const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs); + + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + uint16_t pwm_limited[MAX_ACTUATORS] {}; + + pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, + _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + + /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ + if (_armed.force_failsafe) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + pwm_limited[i] = _failsafe_pwm[i]; + } + } + + /* overwrite outputs in case of lockdown or parachute triggering with disarmed PWM values */ + if (_armed.lockdown || _armed.manual_lockdown) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + pwm_limited[i] = _disarmed_pwm[i]; + } + } + + /* apply _motor_ordering */ + reorder_outputs(pwm_limited); + + /* output to the servos */ + if (_pwm_initialized && !_test_mode) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + } + + /* Trigger all timer's channels in Oneshot mode to fire + * the oneshots with updated values. + */ + if (n_updates > 0 && !_test_mode) { + up_pwm_update(); + } + + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = mixed_num_outputs; + + // zero unused outputs + for (size_t i = 0; i < mixed_num_outputs; ++i) { + actuator_outputs.output[i] = pwm_limited[i]; + } + + actuator_outputs.timestamp = hrt_absolute_time(); + _outputs_pub.publish(actuator_outputs); + + /* publish mixer status */ + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = _mixers->get_saturation_status(); + + if (saturation_status.flags.valid) { + multirotor_motor_limits_s motor_limits{}; + motor_limits.timestamp = hrt_absolute_time(); + motor_limits.saturation_status = saturation_status.value; + + _to_mixer_status.publish(motor_limits); + } + + _mixers->set_airmode(_airmode); + + // use first valid timestamp_sample for latency tracking + for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + const bool required = _groups_required & (1 << i); + const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; + + if (required && (timestamp_sample > 0)) { + perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); + break; + } + } + } + + _cycle_timestamp = hrt_absolute_time(); #ifdef GPIO_BTN_SAFETY - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { - _last_safety_check = _cycle_timestamp; + if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { + _last_safety_check = _cycle_timestamp; - /** - * Get and handle the safety status at 10Hz - */ - struct safety_s safety = {}; + /** + * Get and handle the safety status at 10Hz + */ + struct safety_s safety = {}; - if (!_safety_disabled) { - /* read safety switch input and control safety switch LED at 10Hz */ - safety_check_button(); - } - - /* Make the safety button flash anyway, no matter if it's used or not. */ - flash_safety_button(); - - safety.timestamp = hrt_absolute_time(); - safety.safety_switch_available = true; - safety.safety_off = _safety_btn_off; - - /* lazily publish the safety status */ - if (_to_safety != nullptr) { - orb_publish(ORB_ID(safety), _to_safety, &safety); - - } else { - int instance; - _to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT); - } + if (!_safety_disabled) { + /* read safety switch input and control safety switch LED at 10Hz */ + safety_check_button(); } + + /* Make the safety button flash anyway, no matter if it's used or not. */ + flash_safety_button(); + + safety.timestamp = hrt_absolute_time(); + safety.safety_switch_available = true; + safety.safety_off = _safety_btn_off; + + /* lazily publish the safety status */ + _to_safety.publish(safety); } + } #endif - /* check safety button state */ - bool updated = false; - orb_check(_safety_sub, &updated); + // check safety button state + if (_safety_sub.updated()) { + safety_s safety; - if (updated) { - safety_s safety; - - if (orb_copy(ORB_ID(safety), _safety_sub, &safety) == 0) { - _safety_off = !safety.safety_switch_available || safety.safety_off; - } - } - - /* check arming state */ - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* Update the armed status and check that we're not locked down. - * We also need to arm throttle for the ESC calibration. */ - _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || - (_safety_off && _armed.in_esc_calibration_mode); - } - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _armed.armed || _num_disarmed_set > 0 || _armed.in_esc_calibration_mode; - - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - - update_pwm_out_state(pwm_on); - } - - orb_check(_param_sub, &updated); - - if (updated) { - this->update_params(); - } - - 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)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); - } - - break; + if (_safety_sub.copy(&safety)) { + _safety_off = !safety.safety_switch_available || safety.safety_off; } } + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = _armed.armed || (_num_disarmed_set > 0) || _armed.in_esc_calibration_mode; + + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + update_pwm_out_state(pwm_on); + } + + if (_param_sub.updated()) { + update_params(); + } + + // check at end of cycle (subscribe() can potentially change to a different WorkQueue thread) + if ((_groups_subscribed != _groups_required) || (_current_update_rate == 0)) { + subscribe(); + } + + perf_end(_cycle_perf); } void PX4FMU::update_params() { parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + _param_sub.update(&pupdate); update_pwm_rev_mask(); update_pwm_trims(); @@ -1395,10 +1209,7 @@ void PX4FMU::update_params() int -PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +PX4FMU::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -1547,7 +1358,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1580,7 +1391,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) */ _num_failsafe_set = 0; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { if (_failsafe_pwm[i] > 0) { _num_failsafe_set++; } @@ -1592,11 +1403,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_FAILSAFE_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { pwm->values[i] = _failsafe_pwm[i]; } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; break; } @@ -1604,7 +1415,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1635,7 +1446,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) */ _num_disarmed_set = 0; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { if (_disarmed_pwm[i] > 0) { _num_disarmed_set++; } @@ -1647,11 +1458,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { pwm->values[i] = _disarmed_pwm[i]; } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; break; } @@ -1659,7 +1470,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1691,11 +1502,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { pwm->values[i] = _min_pwm[i]; } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; arg = (unsigned long)&pwm; break; } @@ -1704,7 +1515,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1729,11 +1540,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { pwm->values[i] = _max_pwm[i]; } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; arg = (unsigned long)&pwm; break; } @@ -1742,7 +1553,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { PX4_DEBUG("error: too many trim values: %d", pwm->channel_count); ret = -EINVAL; break; @@ -2129,6 +1940,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) delete _mixers; _mixers = nullptr; _groups_required = 0; + ScheduleNow(); // run to clear schedule and callbacks } break; @@ -2150,7 +1962,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - PX4_DEBUG("mixer load failed with %d", ret); + PX4_ERR("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -2164,6 +1976,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } } + ScheduleNow(); // run to clear schedule and callbacks + break; } @@ -2185,7 +1999,7 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[_MAX_ACTUATORS]; + uint16_t values[MAX_ACTUATORS]; #if BOARD_HAS_PWM == 0 return 0; @@ -2196,8 +2010,8 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) count = BOARD_HAS_PWM; } - if (count > _MAX_ACTUATORS) { - count = _MAX_ACTUATORS; + if (count > MAX_ACTUATORS) { + count = MAX_ACTUATORS; } // allow for misaligned values @@ -2219,9 +2033,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) } void -PX4FMU::reorder_outputs(uint16_t values[_MAX_ACTUATORS]) +PX4FMU::reorder_outputs(uint16_t values[MAX_ACTUATORS]) { - if (_MAX_ACTUATORS < 4) { + if (MAX_ACTUATORS < 4) { return; } @@ -2805,12 +2619,6 @@ PX4FMU::fake(int argc, char *argv[]) return 0; } -PX4FMU *PX4FMU::instantiate(int argc, char *argv[]) -{ - // No arguments to parse. We also know that we should run as task - return new PX4FMU(true); -} - int PX4FMU::custom_command(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; @@ -3040,11 +2848,7 @@ mixer files. int PX4FMU::print_status() { - PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue")); - - if (!_run_as_task) { - PX4_INFO("Max update rate: %i Hz", _current_update_rate); - } + PX4_INFO("Max update rate: %i Hz", _current_update_rate); #ifdef GPIO_BTN_SAFETY if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { @@ -3096,6 +2900,10 @@ int PX4FMU::print_status() PX4_INFO("PWM Mode: %s", mode_str); } + perf_print_counter(_cycle_perf); + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_control_latency_perf); + return 0; } diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index df4ed00bda..30f2867c5a 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-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 @@ -50,19 +50,3 @@ * @group PWM Outputs */ PARAM_DEFINE_INT32(MOT_ORDERING, 0); - -/** - * Run the FMU as a task to reduce latency - * - * If true, the FMU will run in a separate task instead of on the work queue. - * Set this if low latency is required, for example for racing. - * - * This is a trade-off between RAM usage and latency: running as a task, it - * requires a separate stack and directly polls on the control topics, whereas - * running on the work queue, it runs at a fixed update rate. - * - * @boolean - * @reboot_required true - * @group System - */ -PARAM_DEFINE_INT32(SYS_FMU_TASK, 1); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 96c850d513..1471b64628 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -242,7 +242,7 @@ VehicleAngularVelocity::Run() rates -= _bias; vehicle_angular_velocity_s angular_velocity; - angular_velocity.timestamp_sample = sensor_data.timestamp; + angular_velocity.timestamp_sample = sensor_data.timestamp_sample; rates.copyTo(angular_velocity.xyz); angular_velocity.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index d282b92a5e..cfee84e7bf 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -124,7 +124,7 @@ public: void call() override { // schedule immediately if no interval, otherwise check time elapsed - if ((_interval_us == 0) || (hrt_elapsed_time(&_last_update) >= _interval_us)) { + if ((_interval_us == 0) || (hrt_elapsed_time_atomic(&_last_update) >= _interval_us)) { _work_item->ScheduleNow(); } } diff --git a/src/platforms/common/px4_work_queue/WorkItem.hpp b/src/platforms/common/px4_work_queue/WorkItem.hpp index 2135d0367c..756a5d453f 100644 --- a/src/platforms/common/px4_work_queue/WorkItem.hpp +++ b/src/platforms/common/px4_work_queue/WorkItem.hpp @@ -59,6 +59,14 @@ public: protected: + /** + * Initialize WorkItem given a WorkQueue config. This call + * can also be used to switch to a different WorkQueue. + * NOTE: Caller is responsible for synchronization. + * + * @param config The WorkQueue configuration (see WorkQueueManager.hpp). + * @return true if initialization was successful + */ bool Init(const wq_config_t &config); void Deinit();