diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index cf0b8817df..884530668d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -165,7 +165,7 @@ public: unsigned num_outputs, unsigned num_control_groups_updated) override; private: - MixingOutput _mixing_output{*this, true}; + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, true}; static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; @@ -263,6 +263,8 @@ PX4FMU::init() // Getting initial parameter values update_params(); + ScheduleNow(); + return 0; } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 20b195521c..2c791776b2 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -39,7 +39,8 @@ using namespace time_literals; -MixingOutput::MixingOutput(OutputModuleInterface &interface, bool support_esc_calibration, bool ramp_up) +MixingOutput::MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, + bool support_esc_calibration, bool ramp_up) : ModuleParams(&interface), _control_subs{ {&interface, ORB_ID(actuator_controls_0)}, @@ -47,6 +48,7 @@ MixingOutput::MixingOutput(OutputModuleInterface &interface, bool support_esc_ca {&interface, ORB_ID(actuator_controls_2)}, {&interface, ORB_ID(actuator_controls_3)} }, +_scheduling_policy(scheduling_policy), _support_esc_calibration(support_esc_calibration), _interface(interface), _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) @@ -113,45 +115,46 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) // must be locked to potentially change WorkQueue lock(); - // first clear everything - _interface.ScheduleClear(); - unregister(); + if (_scheduling_policy == SchedulingPolicy::Auto) { + // first clear everything + _interface.ScheduleClear(); + unregister(); - // 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 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 (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) { - if (_interface.ChangeWorkQeue(px4::wq_configurations::rate_ctrl)) { - // let the new WQ handle the subscribe update - _wq_switched = true; - _interface.ScheduleNow(); - unlock(); - return false; + if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) { + if (_interface.ChangeWorkQeue(px4::wq_configurations::rate_ctrl)) { + // let the new WQ handle the subscribe update + _wq_switched = true; + _interface.ScheduleNow(); + unlock(); + return false; + } + } + + // register callback to all required actuator control groups + 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); + } + } + } + + // if nothing required keep periodic schedule (so the module can update other things) + if (_groups_required == 0) { + // TODO: this might need to be configurable depending on the module + _interface.ScheduleOnInterval(100_ms); } } _groups_subscribed = _groups_required; - - // subscribe to all required actuator control groups - 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); - } - } - } - setMaxTopicUpdateRate(_max_topic_update_interval_us); - // if nothing required keep periodic schedule (so the module can update other things) - if (_groups_required == 0) { - // TODO: this might need to be configurable depending on the module - _interface.ScheduleOnInterval(100_ms); - } - PX4_DEBUG("_groups_required 0x%08x", _groups_required); PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed); diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index f0704e9bb6..46f010c690 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -72,7 +72,7 @@ public: * @class MixingOutput * This handles the mixing, arming/disarming and all subscriptions required for that. * - * It also drives the scheduling of the OutputModuleInterface (via uORB callbacks + * It can also drive the scheduling of the OutputModuleInterface (via uORB callbacks * to reduce output latency). */ class MixingOutput : public ModuleParams @@ -80,13 +80,20 @@ class MixingOutput : public ModuleParams public: static constexpr int MAX_ACTUATORS = OutputModuleInterface::MAX_ACTUATORS; + enum class SchedulingPolicy { + Disabled, ///< Do not drive scheduling (the module needs to call ScheduleOnInterval() for example) + Auto ///< Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) + }; + /** * Contructor * @param interface Parent module for scheduling, parameter updates and callbacks + * @param scheduling_policy * @param support_esc_calibration true if the output module supports ESC calibration via max, then min setting * @param ramp_up true if motor ramp up from disarmed to min upon arming is wanted */ - MixingOutput(OutputModuleInterface &interface, bool support_esc_calibration, bool ramp_up = true); + MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, + bool support_esc_calibration, bool ramp_up = true); ~MixingOutput(); @@ -212,8 +219,9 @@ private: MixerGroup *_mixers{nullptr}; uint32_t _groups_required{0}; - uint32_t _groups_subscribed{0}; + uint32_t _groups_subscribed{1u << 31}; ///< initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS) + const SchedulingPolicy _scheduling_policy; const bool _support_esc_calibration; bool _wq_switched{false};