mavlink: move HIL_ACTUATOR_CONTROLS to separate streams header

src/modules/mavlink/mavlink_messages.cpp

@@ -111,6 +111,7 @@ using matrix::wrap_2pi;
 #include "streams/GPS_GLOBAL_ORIGIN.hpp"
 #include "streams/GPS_STATUS.hpp"
 #include "streams/HIGH_LATENCY2.hpp"
+#include "streams/HIL_ACTUATOR_CONTROLS.hpp"
 #include "streams/HIL_STATE_QUATERNION.hpp"
 #include "streams/MANUAL_CONTROL.hpp"
 #include "streams/MOUNT_ORIENTATION.hpp"
@@ -3172,162 +3173,6 @@ protected:
 	}
 };
 
-class MavlinkStreamHILActuatorControls : public MavlinkStream
-{
-public:
-	const char *get_name() const override
-	{
-		return MavlinkStreamHILActuatorControls::get_name_static();
-	}
-
-	static constexpr const char *get_name_static()
-	{
-		return "HIL_ACTUATOR_CONTROLS";
-	}
-
-	static constexpr uint16_t get_id_static()
-	{
-		return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS;
-	}
-
-	uint16_t get_id() override
-	{
-		return get_id_static();
-	}
-
-	static MavlinkStream *new_instance(Mavlink *mavlink)
-	{
-		return new MavlinkStreamHILActuatorControls(mavlink);
-	}
-
-	unsigned get_size() override
-	{
-		return _act_sub.advertised() ? MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
-
-private:
-	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
-
-	/* do not allow top copying this class */
-	MavlinkStreamHILActuatorControls(MavlinkStreamHILActuatorControls &) = delete;
-	MavlinkStreamHILActuatorControls &operator = (const MavlinkStreamHILActuatorControls &) = delete;
-
-protected:
-	explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink)
-	{}
-
-	bool send() override
-	{
-		actuator_outputs_s act;
-
-		if (_act_sub.update(&act)) {
-			vehicle_status_s status{};
-			_status_sub.copy(&status);
-
-			if ((status.timestamp > 0) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
-				/* translate the current system state to mavlink state and mode */
-				uint8_t mavlink_state;
-				uint8_t mavlink_base_mode;
-				uint32_t mavlink_custom_mode;
-				mavlink_hil_actuator_controls_t msg = {};
-
-				get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-				const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
-
-				unsigned system_type = _mavlink->get_system_type();
-
-				/* scale outputs depending on system type */
-				if (system_type == MAV_TYPE_QUADROTOR ||
-				    system_type == MAV_TYPE_HEXAROTOR ||
-				    system_type == MAV_TYPE_OCTOROTOR ||
-				    system_type == MAV_TYPE_VTOL_DUOROTOR ||
-				    system_type == MAV_TYPE_VTOL_QUADROTOR ||
-				    system_type == MAV_TYPE_VTOL_RESERVED2) {
-
-					/* multirotors: set number of rotor outputs depending on type */
-
-					unsigned n;
-
-					switch (system_type) {
-					case MAV_TYPE_QUADROTOR:
-						n = 4;
-						break;
-
-					case MAV_TYPE_HEXAROTOR:
-						n = 6;
-						break;
-
-					case MAV_TYPE_VTOL_DUOROTOR:
-						n = 2;
-						break;
-
-					case MAV_TYPE_VTOL_QUADROTOR:
-						n = 4;
-						break;
-
-					case MAV_TYPE_VTOL_RESERVED2:
-						n = 8;
-						break;
-
-					default:
-						n = 8;
-						break;
-					}
-
-					for (unsigned i = 0; i < 16; i++) {
-						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-							if (i < n) {
-								/* scale PWM out 900..2100 us to 0..1 for rotors */
-								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-
-							} else {
-								/* scale PWM out 900..2100 us to -1..1 for other channels */
-								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
-							}
-
-						} else {
-							/* send 0 when disarmed and for disabled channels */
-							msg.controls[i] = 0.0f;
-						}
-					}
-
-				} else {
-					/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
-
-					for (unsigned i = 0; i < 16; i++) {
-						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-							if (i != 3) {
-								/* scale PWM out 900..2100 us to -1..1 for normal channels */
-								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
-
-							} else {
-								/* scale PWM out 900..2100 us to 0..1 for throttle */
-								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-							}
-
-						} else {
-							/* set 0 for disabled channels */
-							msg.controls[i] = 0.0f;
-						}
-					}
-				}
-
-				msg.time_usec = hrt_absolute_time();
-				msg.mode = mavlink_base_mode;
-				msg.flags = 0;
-
-				mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
-
-				return true;
-			}
-		}
-
-		return false;
-	}
-};
-
 class MavlinkStreamCameraCapture : public MavlinkStream
 {
 public:
@@ -3438,7 +3283,9 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamHomePosition>(),
 	create_stream_list_item<MavlinkStreamServoOutputRaw<0> >(),
 	create_stream_list_item<MavlinkStreamServoOutputRaw<1> >(),
+#if defined(HIL_ACTUATOR_CONTROLS_HPP)
 	create_stream_list_item<MavlinkStreamHILActuatorControls>(),
+#endif // HIL_ACTUATOR_CONTROLS_HPP
 #if defined(POSITION_TARGET_GLOBAL_INT_HPP)
 	create_stream_list_item<MavlinkStreamPositionTargetGlobalInt>(),
 #endif // POSITION_TARGET_GLOBAL_INT_HPP

src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp

@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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 HIL_ACTUATOR_CONTROLS_HPP
+#define HIL_ACTUATOR_CONTROLS_HPP
+
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+
+class MavlinkStreamHILActuatorControls : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHILActuatorControls(mavlink); }
+
+	static constexpr const char *get_name_static() { return "HIL_ACTUATOR_CONTROLS"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _act_sub.advertised() ? MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+
+	bool send() override
+	{
+		actuator_outputs_s act;
+
+		if (_act_sub.update(&act)) {
+			mavlink_hil_actuator_controls_t msg{};
+			msg.time_usec = act.timestamp;
+
+			static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
+
+			unsigned system_type = _mavlink->get_system_type();
+
+			/* scale outputs depending on system type */
+			if (system_type == MAV_TYPE_QUADROTOR ||
+			    system_type == MAV_TYPE_HEXAROTOR ||
+			    system_type == MAV_TYPE_OCTOROTOR ||
+			    system_type == MAV_TYPE_VTOL_DUOROTOR ||
+			    system_type == MAV_TYPE_VTOL_QUADROTOR ||
+			    system_type == MAV_TYPE_VTOL_RESERVED2) {
+
+				/* multirotors: set number of rotor outputs depending on type */
+
+				unsigned n;
+
+				switch (system_type) {
+				case MAV_TYPE_QUADROTOR:
+					n = 4;
+					break;
+
+				case MAV_TYPE_HEXAROTOR:
+					n = 6;
+					break;
+
+				case MAV_TYPE_VTOL_DUOROTOR:
+					n = 2;
+					break;
+
+				case MAV_TYPE_VTOL_QUADROTOR:
+					n = 4;
+					break;
+
+				case MAV_TYPE_VTOL_RESERVED2:
+					n = 8;
+					break;
+
+				default:
+					n = 8;
+					break;
+				}
+
+				for (unsigned i = 0; i < 16; i++) {
+					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+						if (i < n) {
+							/* scale PWM out 900..2100 us to 0..1 for rotors */
+							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+
+						} else {
+							/* scale PWM out 900..2100 us to -1..1 for other channels */
+							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+						}
+
+					} else {
+						/* send 0 when disarmed and for disabled channels */
+						msg.controls[i] = 0.0f;
+					}
+				}
+
+			} else {
+				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+				for (unsigned i = 0; i < 16; i++) {
+					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+						if (i != 3) {
+							/* scale PWM out 900..2100 us to -1..1 for normal channels */
+							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+
+						} else {
+							/* scale PWM out 900..2100 us to 0..1 for throttle */
+							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+						}
+
+					} else {
+						/* set 0 for disabled channels */
+						msg.controls[i] = 0.0f;
+					}
+				}
+			}
+
+			// mode (MAV_MODE_FLAG)
+			msg.mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+			vehicle_control_mode_s control_mode;
+
+			if (_vehicle_control_mode_sub.copy(&control_mode)) {
+				if (control_mode.flag_control_auto_enabled) {
+					msg.mode |= MAV_MODE_FLAG_AUTO_ENABLED;
+				}
+
+				if (control_mode.flag_control_manual_enabled) {
+					msg.mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+				}
+
+				if (control_mode.flag_control_attitude_enabled) {
+					msg.mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+				}
+			}
+
+			vehicle_status_s status;
+
+			if (_vehicle_status_sub.copy(&status)) {
+				if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+					msg.mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+				}
+
+				if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+					msg.mode |= MAV_MODE_FLAG_HIL_ENABLED;
+				}
+
+				if (status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+					msg.mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+				}
+			}
+
+			msg.flags = 0;
+
+			mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // HIL_ACTUATOR_CONTROLS_HPP