From b1850a316bc3ba7eb9c77f52cdf16035719e67c2 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Tue, 1 Sep 2015 09:35:43 +0200
Subject: [PATCH] support sitl for planes

---
 src/modules/simulator/simulator.h           |  9 ++++-
 src/modules/simulator/simulator_mavlink.cpp | 44 +++++++++++++--------
 2 files changed, 35 insertions(+), 18 deletions(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 0009d9e1de..dbb3d5b783 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -43,6 +43,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_baro.h>
@@ -217,10 +218,12 @@ private:
 	_actuator_outputs_sub(-1),
 	_vehicle_attitude_sub(-1),
 	_manual_sub(-1),
+	_vehicle_status_sub(-1),
 	_rc_input{},
 	_actuators{},
 	_attitude{},
-	_manual{}
+	_manual{},
+	_vehicle_status{}
 #endif
 	{}
 	~Simulator() { _instance=NULL; }
@@ -255,14 +258,16 @@ private:
 	int _actuator_outputs_sub;
 	int _vehicle_attitude_sub;
 	int _manual_sub;
+	int _vehicle_status_sub;
 
 	// uORB data containers
 	struct rc_input_values _rc_input;
 	struct actuator_outputs_s _actuators;
 	struct vehicle_attitude_s _attitude;
 	struct manual_control_setpoint_s _manual;
+	struct vehicle_status_s _vehicle_status;
 
-	void poll_actuators();
+	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
 	void pollForMAVLinkMessages(bool publish);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 7b6b4594f0..d6d5a22b3e 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -71,26 +71,33 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 	// for now we only support quadrotors
 	unsigned n = 4;
 
-	for (unsigned i = 0; i < 8; i++) {
-		if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
-			if (i < n) {
-				// scale PWM out 900..2100 us to 0..1 for rotors */
-				out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+	if (_vehicle_status.is_rotary_wing) {
+		for (unsigned i = 0; i < 8; i++) {
+			if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
+				if (i < n) {
+					// scale PWM out 900..2100 us to 0..1 for rotors */
+					out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+
+				} else {
+					// scale PWM out 900..2100 us to -1..1 for other channels */
+					out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+				}
 
 			} else {
-				// scale PWM out 900..2100 us to -1..1 for other channels */
-				out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+				// send 0 when disarmed and for disabled channels */
+				out[i] = 0.0f;
 			}
-
-		} 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;
 		}
 	}
 
 	actuator_msg.time_usec = hrt_absolute_time();
 	actuator_msg.roll_ailerons = out[0];
-	actuator_msg.pitch_elevator = out[1];
+	actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1];
 	actuator_msg.yaw_rudder = out[2];
 	actuator_msg.throttle = out[3];
 	actuator_msg.aux1 = out[4];
@@ -270,14 +277,18 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	}
 }
 
-void Simulator::poll_actuators() {
+void Simulator::poll_topics() {
 	// copy new actuator data if available
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
-	if(updated) {
-	  //PX4_WARN("Received actuator_output0 orb_topic");
+	if (updated) {
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
+
+	orb_check(_vehicle_status_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+	}
 }
 
 void *Simulator::sending_trampoline(void *) {
@@ -310,7 +321,7 @@ void Simulator::send() {
 
 		if (fds[0].revents & POLLIN) {
 			// got new data to read, update all topics
-			poll_actuators();
+			poll_topics();
 			send_controls();
 		}
 	}
@@ -419,6 +430,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 	// subscribe to topics
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	// got data from simulator, now activate the sending thread
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);