diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9da33c6a82..caf58a4b32 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -87,7 +87,7 @@ #include #include #include - +#include #ifdef HRT_PPM_CHANNEL # include @@ -230,6 +230,7 @@ private: bool _safety_off; 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) @@ -343,9 +344,9 @@ PX4FMU::PX4FMU() : _safety_off(false), _safety_disabled(false), _to_safety(nullptr), + _to_mixer_status(nullptr), _mot_t_max(0.0f), _ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat")) - { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -1166,6 +1167,18 @@ PX4FMU::cycle() float outputs[_max_actuators]; num_outputs = _mixers->mix(outputs, num_outputs, NULL); + /* publish mixer status */ + multirotor_motor_limits_s multirotor_motor_limits = {}; + multirotor_motor_limits.saturation_status = _mixers->get_saturation_status(); + + if (_to_mixer_status == nullptr) { + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits); + + } else { + orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits); + + } + /* disable unused ports by setting their output to NaN */ for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { if (i >= num_outputs) {