From f11d42aab365809be3c765927392987fc6aca27e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 09:34:20 +0200
Subject: [PATCH] Simulator MAVLink: Forward port cleanly

---
 src/modules/simulator/simulator_mavlink.cpp | 66 ++++++---------------
 1 file changed, 19 insertions(+), 47 deletions(-)

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 87b773c651..2c3fe4f155 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -72,60 +72,41 @@ static socklen_t _addrlen = sizeof(_srcaddr);
 static bool actuators_on = false;
 static hrt_abstime batt_sim_start = 0;
 
+const unsigned mode_flag_armed = 128; // following MAVLink spec
+const unsigned mode_flag_custom = 1;
+
 using namespace simulator;
 
 void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 {
+	// reset state
+	memset(&actuator_msg, 0, sizeof(actuator_msg));
+	actuator_msg.time_usec = hrt_absolute_time();
+
 	float out[8] = {};
+	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
 	const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
 
-	// for now we only support quadrotors
-	unsigned n = 4;
+	for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
+		// scale PWM out 900..2100 us to -1..1 */
+		out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
-	if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) {
-		for (unsigned i = 0; i < 8; i++) {
-			if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) {
-				if (i < n) {
-					// scale PWM out 900..2100 us to 0..1 for rotors */
-					out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-
-				} else {
-					// scale PWM out 900..2100 us to -1..1 for other channels */
-					out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
-				}
-
-			} else {
-				// send 0 when disarmed and for disabled channels */
-				out[i] = 0.0f;
-			}
-		}
-
-	} else {
-		// convert back to range [-1, 1]
-		for (unsigned i = 0; i < 8; i++) {
-			out[i] = (_actuators.output[i] - 1500) / 600.0f;
+		if (!PX4_ISFINITE(out[i])) {
+			out[i] = -1.0f;
 		}
 	}
 
-	// if vehicle status has not yet been updated, set actuator commands to zero
-	// this is to prevent the simulation getting into a bad state
-	if (_vehicle_status.timestamp == 0) {
-		memset(out, 0, sizeof(out));
-	}
-
-	actuators_on = (out[3] > 0.1f);
-
-	actuator_msg.time_usec = hrt_absolute_time();
 	actuator_msg.roll_ailerons = out[0];
-	actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol)  ? out[1] : -out[1];
+	actuator_msg.pitch_elevator = out[1];
 	actuator_msg.yaw_rudder = out[2];
 	actuator_msg.throttle = out[3];
 	actuator_msg.aux1 = out[4];
 	actuator_msg.aux2 = out[5];
-	actuator_msg.aux3 = _actuators.output[6] > PWM_DEFAULT_MIN / 2 ? out[6] : -1.0f;;
+	actuator_msg.aux3 = out[6];
 	actuator_msg.aux4 = out[7];
-	actuator_msg.mode = 0; // need to put something here
+	actuator_msg.mode = mode_flag_custom;
+	actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
 	actuator_msg.nav_mode = 0;
 }
 
@@ -143,17 +124,6 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->channel_count = rc_channels->chancount;
 	rc->rssi = rc_channels->rssi;
 
-	/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
-			 rc_channels->chan1_raw,
-			 rc_channels->chan2_raw,
-			 rc_channels->chan3_raw,
-			 rc_channels->chan4_raw,
-			 rc_channels->chan5_raw,
-			 rc_channels->chan6_raw,
-			 rc_channels->chan7_raw,
-			 rc_channels->chan8_raw);
-	*/
-
 	rc->values[0] = rc_channels->chan1_raw;
 	rc->values[1] = rc_channels->chan2_raw;
 	rc->values[2] = rc_channels->chan3_raw;
@@ -552,6 +522,8 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
 			// send hearbeat
 			mavlink_heartbeat_t hb = {};
+			hb.autopilot = 12;
+			hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0;
 			send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200);
 
 			if (len > 0) {