diff options
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 20 |
1 files changed, 20 insertions, 0 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3bd2c06a4..33125699f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -88,6 +88,7 @@ #include <uORB/topics/battery_status.h> #include <uORB/topics/servorail_status.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/multirotor_motor_limits.h> #include <debug.h> @@ -288,6 +289,7 @@ private: orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety + orb_advert_t _to_mixer_status; ///< mixer status flags actuator_outputs_s _outputs; ///< mixed outputs servorail_status_s _servorail_status; ///< servorail status @@ -513,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _to_mixer_status(0), _outputs{}, _servorail_status{}, _primary_pwm_device(false), @@ -1687,6 +1690,8 @@ PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ actuator_outputs_s outputs; + multirotor_motor_limits_s motor_limits; + outputs.timestamp = hrt_absolute_time(); /* get servo values from IO */ @@ -1718,6 +1723,21 @@ PX4IO::io_publish_pwm_outputs() orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } + /* get mixer status flags from IO */ + uint16_t mixer_status; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); + memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); + + if (ret != OK) + return ret; + + /* publish mixer status */ + if(_to_mixer_status == 0) { + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + } else { + orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits); + } + return OK; } |