mavlink: HIL fixes, send 0 when disarmed

1 files changed, 36 insertions, 56 deletions

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 8be7d76d7..c22289772 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -778,65 +778,45 @@ protected:
 		uint32_t mavlink_custom_mode;
 		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
-		/* HIL message as per MAVLink spec */
+		/* set number of valid outputs depending on vehicle type */
+		unsigned n;
+		switch (mavlink_system.type) {
+		case MAV_TYPE_QUADROTOR:
+			n = 4;
+			break;
+
+		case MAV_TYPE_HEXAROTOR:
+			n = 6;
+			break;
+
+		default:
+			n = 8;
+			break;
+		}
 
 		/* scale / assign outputs depending on system type */
-		if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-			mavlink_msg_hil_controls_send(_channel,
-						      hrt_absolute_time(),
-						      ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-						      -1,
-						      -1,
-						      -1,
-						      -1,
-						      mavlink_base_mode,
-						      0);
-
-		} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-			mavlink_msg_hil_controls_send(_channel,
-						      hrt_absolute_time(),
-						      ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-						      -1,
-						      -1,
-						      mavlink_base_mode,
-						      0);
-
-		} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-			mavlink_msg_hil_controls_send(_channel,
-						      hrt_absolute_time(),
-						      ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[6] - 900.0f) / 600.0f) / 2.0f,
-						      ((act->output[7] - 900.0f) / 600.0f) / 2.0f,
-						      mavlink_base_mode,
-						      0);
-
-		} else {
-			mavlink_msg_hil_controls_send(_channel,
-						      hrt_absolute_time(),
-						      (act->output[0] - 1500.0f) / 500.0f,
-						      (act->output[1] - 1500.0f) / 500.0f,
-						      (act->output[2] - 1500.0f) / 500.0f,
-						      (act->output[3] - 1000.0f) / 1000.0f,
-						      (act->output[4] - 1500.0f) / 500.0f,
-						      (act->output[5] - 1500.0f) / 500.0f,
-						      (act->output[6] - 1500.0f) / 500.0f,
-						      (act->output[7] - 1500.0f) / 500.0f,
-						      mavlink_base_mode,
-						      0);
+		float out[8];
+
+		for (unsigned i = 0; i < 8; i++) {
+			if (i < n) {
+				if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+					/* scale fake PWM out 900..1200 us to 0..1*/
+					out[i] = (act->output[i] - 900.0f) / 1200.0f;
+
+				} else {
+					/* send 0 when disarmed */
+					out[i] = 0.0f;
+				}
+			} else {
+				out[i] = -1.0f;
+			}
 		}
+
+		mavlink_msg_hil_controls_send(_channel,
+					      hrt_absolute_time(),
+					      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+					      mavlink_base_mode,
+					      0);
 	}
 };