Fixed bug in HIL message handling, operational with actuator outputs now

1 files changed, 47 insertions, 26 deletions

diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 6342ce979..7a393185f 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -419,7 +419,7 @@ l_actuator_outputs(struct listener *l)
 	/* copy actuator data into local buffer */
 	orb_copy(ids[l->arg], *l->subp, &act_outputs);
 
-	if (gcs_link)
+	if (gcs_link) {
 		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
 					  l->arg /* port number */,
 					  act_outputs.output[0],
@@ -430,6 +430,30 @@ l_actuator_outputs(struct listener *l)
 					  act_outputs.output[5],
 					  act_outputs.output[6],
 					  act_outputs.output[7]);
+
+		/* only send in HIL mode */
+		if (mavlink_hil_enabled) {
+
+			/* translate the current syste state to mavlink state and mode */
+			uint8_t mavlink_state = 0;
+			uint8_t mavlink_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+
+			/* HIL message as per MAVLink spec */
+			mavlink_msg_hil_controls_send(chan,
+				hrt_absolute_time(),
+				act_outputs.output[0],
+				act_outputs.output[1],
+				act_outputs.output[2],
+				act_outputs.output[3],
+				act_outputs.output[4],
+				act_outputs.output[5],
+				act_outputs.output[6],
+				act_outputs.output[7],
+				mavlink_mode,
+				0);
+		}
+	}
 }
 
 void
@@ -459,31 +483,28 @@ l_manual_control_setpoint(struct listener *l)
 void
 l_vehicle_attitude_controls(struct listener *l)
 {
-	struct actuator_outputs_s actuators;
-
-	orb_copy(ORB_ID_VEHICLE_CONTROLS, mavlink_subs.act_0_sub, &actuators);
-
-	/* Only send in HIL mode */
-	if (mavlink_hil_enabled) {
-
-		/* translate the current syste state to mavlink state and mode */
-		uint8_t mavlink_state = 0;
-		uint8_t mavlink_mode = 0;
-		get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
-
-		/* HIL message as per MAVLink spec */
-		mavlink_msg_hil_controls_send(chan,
-			hrt_absolute_time(),
-			actuators.output[0],
-			actuators.output[1],
-			actuators.output[2],
-			actuators.output[3],
-			actuators.output[4],
-			actuators.output[5],
-			actuators.output[6],
-			actuators.output[7],
-			mavlink_mode,
-			0);
+	struct actuator_controls_s actuators;
+
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0       ",
+						   actuators.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1       ",
+						   actuators.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2       ",
+						   actuators.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3       ",
+						   actuators.control[3]);
 	}
 }