diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index ef7054b380..2771c974a6 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -33,6 +33,8 @@ #include "PWMSim.hpp" +#include + PWMSim::PWMSim() : CDev(PWM_OUTPUT0_DEVICE_PATH), _perf_control_latency(perf_alloc(PC_ELAPSED, "pwm_out_sim control latency")) @@ -285,6 +287,19 @@ PWMSim::run() } } + /* 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; + + int instance; + orb_publish_auto(ORB_ID(multirotor_motor_limits), &_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT); + } + /* and publish for anyone that cares to see */ _actuator_outputs.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs); diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 46772adf67..5804a466b0 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -112,6 +112,7 @@ private: actuator_outputs_s _actuator_outputs = {}; orb_advert_t _outputs_pub{nullptr}; + orb_advert_t _mixer_status{nullptr}; unsigned _num_outputs{0};