From 065ec5b2dc8b631f2adb89bb810b09a145bce600 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Mon, 8 Jun 2015 17:21:21 +0200
Subject: [PATCH] no need to send non-controls mavlink messages to jMAVSim
 because we can use mavlink app with udp

---
 src/modules/simulator/simulator.h           |  8 +--
 src/modules/simulator/simulator_mavlink.cpp | 67 ++-------------------
 2 files changed, 8 insertions(+), 67 deletions(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 39491d8c0d..b2ebc880cd 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -266,16 +266,12 @@ private:
 
 	int _fd;
 	unsigned char _buf[200];
-	hrt_abstime _time_last;
-	hrt_abstime _heartbeat_last;
-	hrt_abstime _attitude_last;
-	hrt_abstime _manual_last;
 	struct sockaddr_in _srcaddr;
 	socklen_t _addrlen = sizeof(_srcaddr);
 
-	void poll_topics();
+	void poll_actuators();
 	void handle_message(mavlink_message_t *msg);
-	void send_data();
+	void send_controls();
 	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
 	void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 65b73c2899..94ec3ced23 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -90,47 +90,10 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 	actuator_msg.nav_mode = 0;
 }
 
-void Simulator::send_data() {
+void Simulator::send_controls() {
 	mavlink_hil_controls_t msg;
 	pack_actuator_message(msg);
 	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
-
-	// send heartbeat
-	if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) {
-		_heartbeat_last = hrt_absolute_time();
-		mavlink_heartbeat_t msg;
-		msg.base_mode = 0;
-		msg.custom_mode = 0;
-		msg.autopilot = MAV_AUTOPILOT_PX4;
-		msg.mavlink_version = 3;
-		send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200);
-	}
-
-	// send attitude message
-	if (hrt_elapsed_time(&_attitude_last) > 20000) {
-		_attitude_last = hrt_absolute_time();
-		mavlink_attitude_t msg;
-		msg.time_boot_ms = _attitude.timestamp / 1000;
-		msg.roll = _attitude.roll;
-		msg.pitch = _attitude.pitch;
-		msg.yaw = _attitude.yaw;
-		msg.rollspeed = _attitude.rollspeed;
-		msg.pitchspeed = _attitude.pitchspeed;
-		msg.yawspeed = _attitude.yawspeed;
-		send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200);
-	}
-
-	// send manual control setpoint
-	if (hrt_elapsed_time(&_manual_last) > 200000) {
-		_manual_last = hrt_absolute_time();
-		mavlink_manual_control_t msg;
-		msg.x = _manual.x * 1000;
-		msg.y = _manual.y * 1000;
-		msg.z = _manual.z * 1000;
-		msg.r = _manual.r * 1000;
-		msg.buttons = 0;
-		send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200);
-	}
 }
 
 static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
@@ -278,24 +241,13 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	}
 }
 
-void Simulator::poll_topics() {
-	// copy new data if available
+void Simulator::poll_actuators() {
+	// copy new actuator data if available
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
 	if(updated) {
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
-
-	orb_check(_vehicle_attitude_sub, &updated);
-	if(updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
-	}
-
-	orb_check(_manual_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
-	}
 }
 
 void *Simulator::sending_trampoline(void *) {
@@ -308,11 +260,6 @@ void Simulator::send() {
 	fds[0].fd = _actuator_outputs_sub;
 	fds[0].events = POLLIN;
 
-	_time_last = 0;
-	_heartbeat_last = 0;
-	_attitude_last = 0;
-	_manual_last = 0;
-
 	int pret;
 
 	while(true) {
@@ -333,8 +280,8 @@ void Simulator::send() {
 
 		if (fds[0].revents & POLLIN) {
 			// got new data to read, update all topics
-			poll_topics();
-			send_data();
+			poll_actuators();
+			send_controls();
 		}
 	}
 }
@@ -365,8 +312,6 @@ void Simulator::updateSamples()
 
 	// subscribe to topics
 	_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
-	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
@@ -433,7 +378,7 @@ void Simulator::updateSamples()
 
 	if (fds[0].revents & POLLIN) {
 		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
-		send_data();
+		send_controls();
 	}
 
 	// got data from simulator, now activate the sending thread