diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 90f0dd13f4..a2ed82c3c4 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -146,7 +146,7 @@ public: hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); - void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; void mixerChanged() override; @@ -686,11 +686,11 @@ void DShotOutput::mixerChanged() updateTelemetryNumMotors(); } -void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (!_outputs_on) { - return; + return false; } int requested_telemetry_index = -1; @@ -740,6 +740,8 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS if (stop_motors || num_control_groups_updated > 0) { up_dshot_trigger(); } + + return true; } void diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 2f3ee931ba..4f3e46d8a2 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -79,11 +79,13 @@ PWMSim::Run() _mixing_output.updateSubscriptions(true); } -void +bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - // nothing to do, as we are only interested in the actuator_outputs topic publication + // Nothing to do, as we are only interested in the actuator_outputs topic publication. + // That should only be published once we receive actuator_controls (important for lock-step to work correctly) + return num_control_groups_updated > 0; } int diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 0640896c66..50d1ffbe05 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -66,7 +66,7 @@ public: int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 1166855248..398e325559 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -156,7 +156,7 @@ public: void update_pwm_trims(); - void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; private: @@ -719,11 +719,11 @@ PX4FMU::update_pwm_out_state(bool on) } -void PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { if (_test_mode) { - return; + return false; } /* output to the servos */ @@ -739,6 +739,8 @@ void PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], if (num_control_groups_updated > 0) { up_pwm_update(); } + + return true; } void diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index c547147cc9..4c23049dc7 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -308,9 +308,11 @@ bool MixingOutput::update() unsigned num_motor_test = motorTest(); if (num_motor_test > 0) { - _interface.updateOutputs(false, _current_output_value, num_motor_test, 1); - actuator_outputs_s actuator_outputs{}; - setAndPublishActuatorOutputs(num_motor_test, actuator_outputs); + if (_interface.updateOutputs(false, _current_output_value, num_motor_test, 1)) { + actuator_outputs_s actuator_outputs{}; + setAndPublishActuatorOutputs(num_motor_test, actuator_outputs); + } + checkSafetyButton(); handleCommands(); return true; @@ -375,14 +377,13 @@ bool MixingOutput::update() reorderOutputs(_current_output_value); /* now return the outputs to the driver */ - _interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates); + if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) { + actuator_outputs_s actuator_outputs{}; + setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs); - - actuator_outputs_s actuator_outputs{}; - setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs); - - publishMixerStatus(actuator_outputs); - updateLatencyPerfCounter(actuator_outputs); + publishMixerStatus(actuator_outputs); + updateLatencyPerfCounter(actuator_outputs); + } checkSafetyButton(); handleCommands(); diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3c422fcf7a..be729c7b4f 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -66,7 +66,16 @@ public: OutputModuleInterface(const char *name, const px4::wq_config_t &config) : px4::ScheduledWorkItem(name, config), ModuleParams(nullptr) {} - virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + /** + * Callback to update the (physical) actuator outputs in the driver + * @param stop_motors if true, all motors must be stopped (if false, individual motors + * might still be stopped via outputs[i] == disarmed_value) + * @param outputs individual actuator outputs in range [min, max] or failsafe/disarmed value + * @param num_outputs number of outputs (<= max_num_outputs) + * @param num_control_groups_updated number of actuator_control groups updated + * @return if true, the update got handled, and actuator_outputs can be published + */ + virtual bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; /** called whenever the mixer gets updated/reset */