mavlink: add ACTUATOR_OUTPUT_STATUS stream

Co-authored-by: Lise Prunier <lise.prunier@aerialcoboticus.com>

src/modules/mavlink/mavlink_main.cpp

@@ -1622,6 +1622,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ODOMETRY", 30.0f);
 
 		configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
+		configure_stream_local("ACTUATOR_OUTPUT_STATUS", 10.0f);
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream_local("ATTITUDE_TARGET", 10.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -97,6 +97,7 @@
 using matrix::Vector3f;
 using matrix::wrap_2pi;
 
+#include "streams/ACTUATOR_OUTPUT_STATUS.hpp"
 #include "streams/ALTITUDE.hpp"
 #include "streams/ATTITUDE.hpp"
 #include "streams/ATTITUDE_QUATERNION.hpp"
@@ -3011,6 +3012,9 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamScaledPressure<0> >(),
 	// create_stream_list_item<MavlinkStreamScaledPressure<1> >(),
 	// create_stream_list_item<MavlinkStreamScaledPressure<2> >(),
+#if defined(ACTUATOR_OUTPUT_STATUS_HPP)
+	create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),
+#endif // ACTUATOR_OUTPUT_STATUS_HPP
 #if defined(ATTITUDE_HPP)
 	create_stream_list_item<MavlinkStreamAttitude>(),
 #endif // ATTITUDE_HPP

src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef ACTUATOR_OUTPUT_STATUS_HPP
+#define ACTUATOR_OUTPUT_STATUS_HPP
+
+class MavlinkStreamActuatorOutputStatus : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamActuatorOutputStatus::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "ACTUATOR_OUTPUT_STATUS";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamActuatorOutputStatus(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return (_act_output_sub.advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+private:
+	uORB::Subscription _act_output_sub{ORB_ID(actuator_outputs)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamActuatorOutputStatus(MavlinkStreamActuatorOutputStatus &) = delete;
+	MavlinkStreamActuatorOutputStatus &operator = (const MavlinkStreamActuatorOutputStatus &) = delete;
+
+protected:
+	explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send() override
+	{
+		actuator_outputs_s act;
+
+		if (_act_output_sub.update(&act)) {
+			mavlink_actuator_output_status_t msg{};
+
+			msg.time_usec = act.timestamp;
+			msg.active = act.noutputs;
+
+			static size_t actuator_outputs_size = act.noutputs;
+			static constexpr size_t mavlink_actuator_output_status_size = sizeof(msg.actuator) / sizeof(msg.actuator[0]);
+
+			for (unsigned i = 0; i < math::min(actuator_outputs_size, mavlink_actuator_output_status_size); i++) {
+				msg.actuator[i] = act.output[i];
+			}
+
+			mavlink_msg_actuator_output_status_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // ACTUATOR_OUTPUT_STATUS_HPP