From ed7b97c0203fd1a16768222cad6c6f49b0534bd2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 3 Apr 2014 21:13:03 +0200
Subject: commander: don't beep if message is not understood

---
 src/modules/commander/commander.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cf7ba757e..58995fcec 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -592,11 +592,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
-
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
-	}
+	/* silently ignore unsupported commands, maybe they are passed on over mavlink */
+//	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+//		/* already warned about unsupported commands in "default" case */
+//		answer_command(*cmd, result);
+//	}
 
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-- 
cgit v1.2.3


From f17c0b133559dc440a7789caa45767b95de84e7b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 3 Apr 2014 21:15:47 +0200
Subject: mavlink: implemented multicasting between mavlink instances (two
 options: forwarding: forward received messages from self to other mavlink
 instances, passing: send out messages received from other mavlink intances
 over serial

---
 src/modules/mavlink/mavlink_main.cpp     | 197 ++++++++++++++++++++++++++++++-
 src/modules/mavlink/mavlink_main.h       |  34 +++++-
 src/modules/mavlink/mavlink_messages.cpp |   6 +-
 src/modules/mavlink/mavlink_receiver.cpp |   5 +
 4 files changed, 236 insertions(+), 6 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 18df577fe..c5055939e 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -207,10 +207,15 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_pub(-1),
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_uart_fd(-1),
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
+	_message_buffer({}),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -261,7 +266,6 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
-
 }
 
 Mavlink::~Mavlink()
@@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 	return false;
 }
 
+void
+Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	while (inst != nullptr) {
+		/* don't broadcast to itself */
+		if (inst != self) {
+			inst->pass_message(msg);
+		}
+		inst = inst->next;
+	}
+}
+
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -1616,6 +1634,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 	}
 }
 
+int
+Mavlink::message_buffer_init(int size)
+{
+	_message_buffer.size = size;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	_message_buffer.data = (char*)malloc(_message_buffer.size);
+	return (_message_buffer.data == 0) ? ERROR : OK;
+}
+
+void
+Mavlink::message_buffer_destroy()
+{
+	_message_buffer.size = 0;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	free(_message_buffer.data);
+}
+
+int
+Mavlink::message_buffer_count()
+{
+	int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (n < 0) {
+		n += _message_buffer.size;
+	}
+
+	return n;
+}
+
+int
+Mavlink::message_buffer_is_empty()
+{
+	return _message_buffer.read_ptr == _message_buffer.write_ptr;
+}
+
+
+bool
+Mavlink::message_buffer_write(void *ptr, int size)
+{
+	// bytes available to write
+	int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
+
+	if (available < 0) {
+		available += _message_buffer.size;
+	}
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	char *c = (char *) ptr;
+	int n = _message_buffer.size - _message_buffer.write_ptr;	// bytes to end of the buffer
+
+	if (n < size) {
+		// message goes over end of the buffer
+		memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
+		_message_buffer.write_ptr = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	int p = size - n;	// number of bytes to write
+	memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
+	_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
+	return true;
+}
+
+int
+Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (available == 0) {
+		return 0;	// buffer is empty
+	}
+
+	int n = 0;
+
+	if (available > 0) {
+		// read pointer is before write pointer, all available bytes can be read
+		n = available;
+		*is_part = false;
+
+	} else {
+		// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
+		n = _message_buffer.size - _message_buffer.read_ptr;
+		*is_part = _message_buffer.write_ptr > 0;
+	}
+
+	*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
+	return n;
+}
+
+void
+Mavlink::message_buffer_mark_read(int n)
+{
+	_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
+}
+
+void
+Mavlink::pass_message(mavlink_message_t *msg)
+{
+	if (_passing_on) {
+		/* size is 8 bytes plus variable payload */
+		int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
+		pthread_mutex_lock(&_message_buffer_mutex);
+		message_buffer_write(msg, size);
+		pthread_mutex_unlock(&_message_buffer_mutex);
+	}
+}
+
+
+
 int
 Mavlink::task_main(int argc, char *argv[])
 {
@@ -1632,7 +1769,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 			break;
 
+		case 'f':
+			_forwarding_on = true;
+			break;
+
+		case 'p':
+			_passing_on = true;
+			break;
+
 		case 'v':
 			_verbose = true;
 			break;
@@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[])
 	/* initialize mavlink text message buffering */
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
+	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
+	if (_passing_on) {
+		/* initialize message buffer if multiplexing is on */
+		if (OK != message_buffer_init(500)) {
+			errx(1, "can't allocate message buffer, exiting");
+		}
+
+		/* initialize message buffer mutex */
+		pthread_mutex_init(&_message_buffer_mutex, NULL);
+	}
+
 	/* create the device node that's used for sending text log messages, etc. */
 	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
 
@@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
+		/* pass messages from other UARTs */
+		if (_passing_on) {
+
+			bool is_part;
+			void *read_ptr;
+
+			/* guard get ptr by mutex */
+			pthread_mutex_lock(&_message_buffer_mutex);
+			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			pthread_mutex_unlock(&_message_buffer_mutex);
+
+			if (available > 0) {
+				/* write first part of buffer */
+				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				message_buffer_mark_read(available);
+
+				/* write second part of buffer if there is some */
+				if (is_part) {
+					/* guard get ptr by mutex */
+					pthread_mutex_lock(&_message_buffer_mutex);
+					available = message_buffer_get_ptr(&read_ptr, &is_part);
+					pthread_mutex_unlock(&_message_buffer_mutex);
+
+					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+					message_buffer_mark_read(available);
+				}
+			}
+		}
+
+
+
 		perf_end(_loop_perf);
 	}
 
@@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	close(_mavlink_fd);
 
+	if (_passing_on) {
+		message_buffer_destroy();
+		pthread_mutex_destroy(&_message_buffer_mutex);
+	}
 	/* destroy log buffer */
 	mavlink_logbuffer_destroy(&_logbuffer);
 
@@ -2067,7 +2258,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5a118a0ad..4f9a53a5b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -138,6 +138,8 @@ public:
 
 	static bool	instance_exists(const char *device_name, Mavlink *self);
 
+	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+
 	static int get_uart_fd(unsigned index);
 
 	int get_uart_fd();
@@ -153,10 +155,12 @@ public:
 	void		set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE		get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _hil_enabled; };
+	bool		get_hil_enabled() { return _hil_enabled; }
 
 	bool		get_flow_control_enabled() { return _flow_control_enabled; }
 
+	bool		get_forwarding_on() { return _forwarding_on; }
+
 	/**
 	 * Handle waypoint related messages.
 	 */
@@ -234,6 +238,8 @@ private:
 	mavlink_wpm_storage *_wpm;
 
 	bool _verbose;
+	bool _forwarding_on;
+	bool _passing_on;
 	int _uart_fd;
 	int _baudrate;
 	int _datarate;
@@ -252,6 +258,16 @@ private:
 
 	bool		_flow_control_enabled;
 
+	struct mavlink_message_buffer {
+		int write_ptr;
+		int read_ptr;
+		int size;
+		char *data;
+	};
+	mavlink_message_buffer _message_buffer;
+
+	pthread_mutex_t _message_buffer_mutex;
+
 	/**
 	 * Send one parameter.
 	 *
@@ -315,6 +331,22 @@ private:
 	int configure_stream(const char *stream_name, const float rate);
 	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
+	int message_buffer_init(int size);
+
+	void message_buffer_destroy();
+
+	int message_buffer_count();
+
+	int message_buffer_is_empty();
+
+	bool message_buffer_write(void *ptr, int size);
+
+	int message_buffer_get_ptr(void **ptr, bool *is_part);
+
+	void message_buffer_mark_read(int n);
+
+	void pass_message(mavlink_message_t *msg);
+
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
 	/**
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 4ca3840d4..2b5d65080 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1265,11 +1265,13 @@ protected:
 		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
-			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+			/* XXX TODO: get param for system ID */
+			mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
 
 		} else {
 			/* send camera capture off */
-			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+			/* XXX TODO: get param for system ID */
+			mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
 		}
 	}
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d7e300670..1581f30d3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -913,6 +913,11 @@ MavlinkReceiver::receive_thread(void *arg)
 
 					/* handle packet with parameter component */
 					_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
+
+					if (_mavlink->get_forwarding_on()) {
+						/* forward any messages to other mavlink instances */
+						Mavlink::forward_message(&msg, _mavlink);
+					}
 				}
 			}
 		}
-- 
cgit v1.2.3


From 4a0c6600887e900932f6888f1b8948816a1f00b4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 21:06:21 +0200
Subject: Do not make minimum airspeed assumptions, as we can trust our digital
 sensor a bit. A blocked pitot also most likely will result in more than just
 6.5 m/s airspeed and so the check is based on a bunch of weak assumptions

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index f139c25f4..522e8c3e6 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -660,14 +660,13 @@ FixedwingAttitudeControl::task_main()
 
 				float airspeed;
 
-				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
+				/* if airspeed is not updating, we assume the normal average speed */
+				if (!isfinite(_airspeed.true_airspeed_m_s) ||
 				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 
 				} else {
-					airspeed = _airspeed.indicated_airspeed_m_s;
+					airspeed = _airspeed.true_airspeed_m_s;
 				}
 
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
-- 
cgit v1.2.3


From ab60b13b6dbcf636b1889d2150d96aff8b26cfc9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Apr 2014 21:26:17 +0200
Subject: fw_att_controller: Forcing actuator scaling to at least minimum speed

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 522e8c3e6..2f84dc963 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -669,8 +669,15 @@ FixedwingAttitudeControl::task_main()
 					airspeed = _airspeed.true_airspeed_m_s;
 				}
 
-				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
-				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+				/*
+				 * For scaling our actuators using anything less than the min (close to stall)
+				 * speed doesn't make any sense - its the strongest reasonable deflection we
+				 * want to do in flight and its the baseline a human pilot would choose.
+				 *
+				 * Forcing the scaling to this value allows reasonable handheld tests.
+				 */
+
+				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
-- 
cgit v1.2.3


From 7c4f1c90dc441e57d9af55beb33bb6d91ece0c90 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 6 Apr 2014 14:57:45 +0400
Subject: mavlink_receiver: fixed bug in manual control publication, minor
 refactoring

---
 src/modules/mavlink/mavlink_receiver.cpp | 60 +++++++++++++++-----------------
 src/modules/mavlink/mavlink_receiver.h   |  1 -
 2 files changed, 29 insertions(+), 32 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d7e300670..3ec40ee0a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			vcmd.source_component = msg->compid;
 			vcmd.confirmation =  cmd_mavlink.confirmation;
 
-			/* check if topic is advertised */
-			if (_cmd_pub <= 0) {
+			if (_cmd_pub < 0) {
 				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 			} else {
-				/* publish */
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
 			}
 		}
@@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
 
-	if (_flow_pub <= 0) {
+	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
 
 	} else {
@@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;
 
-	if (_cmd_pub <= 0) {
+	if (_cmd_pub < 0) {
 		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 	} else {
@@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	vicon_position.pitch = pos.pitch;
 	vicon_position.yaw = pos.yaw;
 
-	if (_vicon_position_pub <= 0) {
+	if (_vicon_position_pub < 0) {
 		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 
 	} else {
@@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub <= 0) {
+		if (_offboard_control_sp_pub < 0) {
 			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
 
 		} else {
@@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	tstatus.rxerrors = rstatus.rxerrors;
 	tstatus.fixed = rstatus.fixed;
 
-	if (_telemetry_status_pub <= 0) {
+	if (_telemetry_status_pub < 0) {
 		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 	} else {
@@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		rc.chan[2].scaled = man.r / 1000.0f;
 		rc.chan[3].scaled = man.z / 1000.0f;
 
-		if (_rc_pub == 0) {
+		if (_rc_pub < 0) {
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 		} else {
@@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		manual.yaw = man.r / 1000.0f;
 		manual.throttle = man.z / 1000.0f;
 
-		if (_manual_pub == 0) {
+		if (_manual_pub < 0) {
 			_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
 		} else {
@@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */
-		if (_sensors_pub > 0) {
-			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 
 		} else {
-			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
 	}
 
@@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 
@@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.fix_type = gps.fix_type;
 	hil_gps.satellites_visible = gps.satellites_visible;
 
-	if (_gps_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 
 	} else {
-		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
 	}
 }
 
@@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.pitchspeed = hil_state.pitchspeed;
 		hil_attitude.yawspeed = hil_state.yawspeed;
 
-		if (_attitude_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 
 		} else {
-			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
 	}
 
@@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 		hil_global_pos.yaw = hil_attitude.yaw;
 
-		if (_global_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
 		} else {
-			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
 		}
 	}
 
@@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
 		hil_local_pos.landed = landed;
 
-		if (_local_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
 		} else {
-			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
 		}
 	}
 
@@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 0a5a1b5c7..72ce4560f 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -120,7 +120,6 @@ private:
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
-	int _manual_sub;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
 	orb_advert_t _attitude_pub;
-- 
cgit v1.2.3


From e4cfdb4f9f30e0eef36f9e18e8d656d57994141e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 6 Apr 2014 19:34:32 +0200
Subject: mavlink: Add manual SP subscription

---
 src/modules/mavlink/mavlink_receiver.h | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 72ce4560f..8ccb2a035 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -138,6 +138,7 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	int _manual_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
-- 
cgit v1.2.3


From 536ff50fe1ddc438283f5f3c66ec9dc3d79d7d83 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 7 Apr 2014 14:38:16 +0200
Subject: mavlink: in normal mode transmit position setpoint and
 roll-pitch-yaw-thrust setpoint

---
 src/modules/mavlink/mavlink_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 18df577fe..16c27baa9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1783,6 +1783,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
 		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
 		break;
 
 	case MAVLINK_MODE_CAMERA:
-- 
cgit v1.2.3


From de3efc0975f1b84cf556bae11bbebe95609dbcdc Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 7 Apr 2014 17:17:56 +0400
Subject: mavlink: publish SYS_STATUS at constant rate, don't look at update()
 result

---
 src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++----------------
 1 file changed, 15 insertions(+), 16 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 4ca3840d4..c89031fcc 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -262,22 +262,21 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		if (status_sub->update(t)) {
-			mavlink_msg_sys_status_send(_channel,
-						    status->onboard_control_sensors_present,
-						    status->onboard_control_sensors_enabled,
-						    status->onboard_control_sensors_health,
-						    status->load * 1000.0f,
-						    status->battery_voltage * 1000.0f,
-						    status->battery_current * 1000.0f,
-						    status->battery_remaining,
-						    status->drop_rate_comm,
-						    status->errors_comm,
-						    status->errors_count1,
-						    status->errors_count2,
-						    status->errors_count3,
-						    status->errors_count4);
-		}
+		status_sub->update(t);
+		mavlink_msg_sys_status_send(_channel,
+						status->onboard_control_sensors_present,
+						status->onboard_control_sensors_enabled,
+						status->onboard_control_sensors_health,
+						status->load * 1000.0f,
+						status->battery_voltage * 1000.0f,
+						status->battery_current * 1000.0f,
+						status->battery_remaining,
+						status->drop_rate_comm,
+						status->errors_comm,
+						status->errors_count1,
+						status->errors_count2,
+						status->errors_count3,
+						status->errors_count4);
 	}
 };
 
-- 
cgit v1.2.3


From a4a12dab337f132bfe51db2ad482273ecfdf0ce6 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:42:19 +0200
Subject: commander: put unsupported warning back in place

---
 src/modules/commander/commander.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 58995fcec..06f7ae6cc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -588,15 +588,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 
 	default:
-		/* warn about unsupported commands */
+		/* Warn about unsupported commands, this makes sense because only commands
+		 * to this component ID (or all) are passed by mavlink. */
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
-	/* silently ignore unsupported commands, maybe they are passed on over mavlink */
-//	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-//		/* already warned about unsupported commands in "default" case */
-//		answer_command(*cmd, result);
-//	}
+	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* already warned about unsupported commands in "default" case */
+		answer_command(*cmd, result);
+	}
 
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-- 
cgit v1.2.3


From bb3792bcdd8edeb24cad7ef0170c76a552f943d5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:42:48 +0200
Subject: mavlink: use LL_FOREACH

---
 src/modules/mavlink/mavlink_main.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c5055939e..58393e013 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -401,14 +401,12 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
 {
-	Mavlink *inst = ::_mavlink_instances;
-
-	while (inst != nullptr) {
-		/* don't broadcast to itself */
+	
+	Mavlink *inst;
+	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
 			inst->pass_message(msg);
 		}
-		inst = inst->next;
 	}
 }
 
-- 
cgit v1.2.3


From 38c3e68976c8dc167b4d1e5d24792401fc7cc7d3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:44:01 +0200
Subject: Send camera command to all, use own sysid

---
 src/modules/mavlink/mavlink_messages.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2b5d65080..1e6bd6f93 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1265,13 +1265,11 @@ protected:
 		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
-			/* XXX TODO: get param for system ID */
-			mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
 
 		} else {
 			/* send camera capture off */
-			/* XXX TODO: get param for system ID */
-			mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
 		}
 	}
 };
-- 
cgit v1.2.3


From 814d3c385cc0132ec9bbdc22f87f35301bbc8f44 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 11:03:23 +0200
Subject: gps driver: fake mode: lower eph and epv values in order to convince
 the commander that the gps signal is valid

---
 src/drivers/gps/gps.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c72f692d7..a902bdf2f 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -282,8 +282,8 @@ GPS::task_main()
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 10.0f;
-			_report.epv_m = 10.0f;
+			_report.eph_m = 3.0f;
+			_report.epv_m = 7.0f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
-- 
cgit v1.2.3


From 134633c7f0ef6ec668ba9c122a52b024c393e51a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 15:15:06 +0200
Subject: mavlink: accessor for _mavlink_fd

---
 src/modules/mavlink/mavlink_main.h | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5a118a0ad..4e6d585ab 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -196,6 +196,8 @@ public:
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
+	int get_mavlink_fd() { return _mavlink_fd; }
+
 protected:
 	Mavlink	*next;
 
-- 
cgit v1.2.3


From a6215b7bda47616018d29b2f7de630deb4984be9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:06:52 +0200
Subject: commander: handle_command: filter commands by sysid and componentid

---
 src/modules/commander/commander.cpp | 5 +++++
 1 file changed, 5 insertions(+)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 69a45a02f..89b8e684d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 	bool ret = false;
 
+	/* only handle commands that are meant to be handled by this system and component */
+	if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) {
+		return false;
+	}
+
 	/* only handle high-priority commands here */
 
 	/* request to set different system mode */
-- 
cgit v1.2.3


From f0d043c06849a2f2f5e07d6f3bdfafc0e4e6f041 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:08:35 +0200
Subject: mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic
 and send mavlink_msg_command_long

---
 src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 45b86a497..4d7b9160d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1273,6 +1273,51 @@ protected:
 	}
 };
 
+class MavlinkStreamCommandLong : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "COMMAND_LONG";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCommandLong();
+	}
+
+private:
+	MavlinkOrbSubscription *vehicle_command_sub;
+	struct vehicle_command_s *vehicle_command;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+		vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (vehicle_command_sub->update(t)) {
+			if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) {
+				mavlink_msg_command_long_send(_channel,
+						vehicle_command->target_system,
+						vehicle_command->target_component,
+						vehicle_command->command,
+						vehicle_command->confirmation,
+						vehicle_command->param1,
+						vehicle_command->param2,
+						vehicle_command->param3,
+						vehicle_command->param4,
+						vehicle_command->param5,
+						vehicle_command->param6,
+						vehicle_command->param7);
+			}
+		}
+	}
+};
+
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+	new MavlinkStreamCommandLong(),
 	nullptr
 };
-- 
cgit v1.2.3


From bccdbde45eb70271bd946ca86173cd8add20cd3d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:29:11 +0200
Subject: commander: handle_command: do not filter command if componentid == 0

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 89b8e684d..ce6de88ef 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	bool ret = false;
 
 	/* only handle commands that are meant to be handled by this system and component */
-	if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) {
+	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
-- 
cgit v1.2.3


From 523606668f8c940357cd9c46b0995035eced7659 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 22:23:18 +0400
Subject: position_estimator_inav: make land detector more sensitive to LANDED
 -> IN AIR transitions

---
 src/modules/position_estimator_inav/position_estimator_inav_main.c   | 2 +-
 src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368fa6ee2..763b87563 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float thrust = armed.armed ? actuator.control[3] : 0.0f;
 
 		if (landed) {
-			if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
+			if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
 				landed = false;
 				landed_time = 0;
 			}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index b71f9472f..dcad5c03b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
-PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
-- 
cgit v1.2.3


From 09093b17daf8f02f87ea689dc73bd35b6cac1542 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 23:28:52 +0400
Subject: mavlink: commands stream implemented

---
 src/modules/mavlink/mavlink_commands.cpp | 73 ++++++++++++++++++++++++++++++++
 src/modules/mavlink/mavlink_commands.h   | 65 ++++++++++++++++++++++++++++
 src/modules/mavlink/mavlink_main.cpp     |  6 +++
 src/modules/mavlink/module.mk            |  3 +-
 4 files changed, 146 insertions(+), 1 deletion(-)
 create mode 100644 src/modules/mavlink/mavlink_commands.cpp
 create mode 100644 src/modules/mavlink/mavlink_commands.h

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 000000000..1c1e097a4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_commands.cpp
+ * Mavlink commands stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_commands.h"
+
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+{
+	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
+}
+
+MavlinkCommandsStream::~MavlinkCommandsStream()
+{
+}
+
+void
+MavlinkCommandsStream::update(const hrt_abstime t)
+{
+	if (_cmd_sub->update(t)) {
+		/* only send commands for other systems/components */
+		if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
+			mavlink_msg_command_long_send(_channel,
+						      _cmd->target_system,
+						      _cmd->target_component,
+						      _cmd->command,
+						      _cmd->confirmation,
+						      _cmd->param1,
+						      _cmd->param2,
+						      _cmd->param3,
+						      _cmd->param4,
+						      _cmd->param5,
+						      _cmd->param6,
+						      _cmd->param7);
+		}
+	}
+}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
new file mode 100644
index 000000000..6255d65df
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_commands.h
+ * Mavlink commands stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_COMMANDS_H_
+#define MAVLINK_COMMANDS_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+
+class Mavlink;
+class MavlinkCommansStream;
+
+#include "mavlink_main.h"
+
+class MavlinkCommandsStream
+{
+private:
+	MavlinkOrbSubscription *_cmd_sub;
+	struct vehicle_command_s *_cmd;
+	mavlink_channel_t _channel;
+
+public:
+	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
+	~MavlinkCommandsStream();
+	void update(const hrt_abstime t);
+};
+
+#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3d897431a..1ed3f4001 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -81,6 +81,7 @@
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 #include "mavlink_rate_limiter.h"
+#include "mavlink_commands.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -1920,6 +1921,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
 
+	MavlinkCommandsStream commands_stream(this, _channel);
+
 	/* add default streams depending on mode and intervals depending on datarate */
 	float rate_mult = _datarate / 1000.0f;
 
@@ -1982,6 +1985,9 @@ Mavlink::task_main(int argc, char *argv[])
 			set_hil_enabled(status->hil_state == HIL_STATE_ON);
 		}
 
+		/* update commands stream */
+		commands_stream.update(t);
+
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f09efa2e6..515fbfadc 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -42,6 +42,7 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
-			mavlink_rate_limiter.cpp
+			mavlink_rate_limiter.cpp \
+			mavlink_commands.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-- 
cgit v1.2.3


From d55e64d1e54542762510387a22897f504c68a5a6 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 23:29:40 +0400
Subject: mavlink: minor comments and formatting fixes

---
 src/modules/mavlink/mavlink_messages.cpp | 2 --
 src/modules/mavlink/mavlink_stream.h     | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 45b86a497..47603b390 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1252,8 +1252,6 @@ protected:
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
-
-
 	}
 
 	void send(const hrt_abstime t)
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 135e1bce0..def40d9ad 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_stream.cpp
+ * @file mavlink_stream.h
  * Mavlink messages stream definition.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
-- 
cgit v1.2.3


From 27884e49bedfdbd0e259bb9b90e757b45bf7fd44 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 8 Apr 2014 21:40:05 +0200
Subject: Revert "Merge pull request #816 from PX4/mavlink_commandlongstream"

This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing
changes made to d55e64d1e54542762510387a22897f504c68a5a6.
---
 src/modules/mavlink/mavlink_messages.cpp | 46 --------------------------------
 1 file changed, 46 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index bb8bba037..47603b390 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1271,51 +1271,6 @@ protected:
 	}
 };
 
-class MavlinkStreamCommandLong : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "COMMAND_LONG";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamCommandLong();
-	}
-
-private:
-	MavlinkOrbSubscription *vehicle_command_sub;
-	struct vehicle_command_s *vehicle_command;
-
-protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-		vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data();
-	}
-
-	void send(const hrt_abstime t)
-	{
-		if (vehicle_command_sub->update(t)) {
-			if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) {
-				mavlink_msg_command_long_send(_channel,
-						vehicle_command->target_system,
-						vehicle_command->target_component,
-						vehicle_command->command,
-						vehicle_command->confirmation,
-						vehicle_command->param1,
-						vehicle_command->param2,
-						vehicle_command->param3,
-						vehicle_command->param4,
-						vehicle_command->param5,
-						vehicle_command->param6,
-						vehicle_command->param7);
-			}
-		}
-	}
-};
-
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
-	new MavlinkStreamCommandLong(),
 	nullptr
 };
-- 
cgit v1.2.3


From 863cd2e838a5e30efd817520c3fceb3847dd18f6 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:11:27 +0200
Subject: Added VICON logging, finally.

---
 src/modules/sdlog2/sdlog2.c          | 30 +++++++++++++++++++-----------
 src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++++
 2 files changed, 31 insertions(+), 11 deletions(-)

(limited to 'src')

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e62b0fafc..83ee93408 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -226,11 +226,11 @@ sdlog2_usage(const char *reason)
 	}
 
 	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
-	     "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
-	     "\t-b\tLog buffer size in KiB, default is 8\n"
-	     "\t-e\tEnable logging by default (if not, can be started by command)\n"
-	     "\t-a\tLog only when armed (can be still overriden by command)\n"
-	     "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
+		 "\t-b\tLog buffer size in KiB, default is 8\n"
+		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
+		 "\t-a\tLog only when armed (can be still overriden by command)\n"
+		 "\t-t\tUse date/time for naming log directories and files\n");
 }
 
 /**
@@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
 
 		main_thread_should_exit = false;
 		deamon_task = task_spawn_cmd("sdlog2",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT - 30,
-					     3000,
-					     sdlog2_thread_main,
-					     (const char **)argv);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT - 30,
+						 3000,
+						 sdlog2_thread_main,
+						 (const char **)argv);
 		exit(0);
 	}
 
@@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
+			struct log_VICON_s log_VICON;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-			// TODO not implemented yet
+            log_msg.msg_type = LOG_VICON_MSG;
+            log_msg.body.log_VICON.x = buf.vicon_pos.x;
+            log_msg.body.log_VICON.y = buf.vicon_pos.y;
+            log_msg.body.log_VICON.z = buf.vicon_pos.z;
+            log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
+            log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
+            log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
+            LOGBUFFER_WRITE_AND_COUNT(VICON);
 		}
 
 		/* --- FLOW --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a1a5856bc..eee5ae590 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -301,6 +301,17 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
+/* --- VICON - VICON POSITION --- */
+#define LOG_VICON_MSG 25
+struct log_VICON_s {
+	float x;
+	float y;
+	float z;
+	float roll;
+	float pitch;
+	float yaw;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
 	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
 	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
+	LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
-- 
cgit v1.2.3


From 284218174cad1817b5dc542ecb1527f6ec58bb2d Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:21:07 +0200
Subject: Tabs tabs tabs

---
 src/modules/sdlog2/sdlog2.c | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 83ee93408..8dfe17228 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-            log_msg.msg_type = LOG_VICON_MSG;
-            log_msg.body.log_VICON.x = buf.vicon_pos.x;
-            log_msg.body.log_VICON.y = buf.vicon_pos.y;
-            log_msg.body.log_VICON.z = buf.vicon_pos.z;
-            log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
-            log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
-            log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
-            LOGBUFFER_WRITE_AND_COUNT(VICON);
+			log_msg.msg_type = LOG_VICON_MSG;
+			log_msg.body.log_VICON.x = buf.vicon_pos.x;
+			log_msg.body.log_VICON.y = buf.vicon_pos.y;
+			log_msg.body.log_VICON.z = buf.vicon_pos.z;
+			log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
+			log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
+			log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(VICON);
 		}
 
 		/* --- FLOW --- */
-- 
cgit v1.2.3


From 5b090a7095a25f31dd040935defa2363a60b3107 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:25:33 +0200
Subject: VICON -> VICN

---
 src/modules/sdlog2/sdlog2_messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index eee5ae590..affd512cc 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
 	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
 	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
-	LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
-- 
cgit v1.2.3


From a39382dc174d646c5ecda8c15c6e0db9c1c4e703 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:28:00 +0200
Subject: VICON -> VICN really this time.

---
 src/modules/sdlog2/sdlog2.c          | 18 +++++++++---------
 src/modules/sdlog2/sdlog2_messages.h |  6 +++---
 2 files changed, 12 insertions(+), 12 deletions(-)

(limited to 'src')

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 8dfe17228..1bf6f1de3 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -833,7 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
-			struct log_VICON_s log_VICON;
+			struct log_VICN_s log_VICN;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-			log_msg.msg_type = LOG_VICON_MSG;
-			log_msg.body.log_VICON.x = buf.vicon_pos.x;
-			log_msg.body.log_VICON.y = buf.vicon_pos.y;
-			log_msg.body.log_VICON.z = buf.vicon_pos.z;
-			log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
-			log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
-			log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
-			LOGBUFFER_WRITE_AND_COUNT(VICON);
+			log_msg.msg_type = LOG_VICN_MSG;
+			log_msg.body.log_VICN.x = buf.vicon_pos.x;
+			log_msg.body.log_VICN.y = buf.vicon_pos.y;
+			log_msg.body.log_VICN.z = buf.vicon_pos.z;
+			log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
+			log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
+			log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(VICN);
 		}
 
 		/* --- FLOW --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index affd512cc..d0585df39 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -301,9 +301,9 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
-/* --- VICON - VICON POSITION --- */
-#define LOG_VICON_MSG 25
-struct log_VICON_s {
+/* --- VICN - VICON POSITION --- */
+#define LOG_VICN_MSG 25
+struct log_VICN_s {
 	float x;
 	float y;
 	float z;
-- 
cgit v1.2.3


From e99291d825cfc89c67cadd8487e568d3b21d218f Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 09:08:37 +0200
Subject: Added vicon stream.

---
 src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 47603b390..37929edac 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -640,6 +640,47 @@ protected:
 };
 
 
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+    const char *get_name()
+    {
+        return "VICON_POSITION_ESTIMATE";
+    }
+
+    MavlinkStream *new_instance()
+    {
+        return new MavlinkStreamViconPositionEstimate();
+    }
+
+private:
+    MavlinkOrbSubscription *pos_sub;
+    struct vehicle_vicon_position_s *pos;
+
+protected:
+    void subscribe(Mavlink *mavlink)
+    {
+        pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+        pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
+    }
+
+    void send(const hrt_abstime t)
+    {
+        if (pos_sub->update(t)) {
+            mavlink_msg_vicon_position_estimate_send(_channel,
+                                pos->timestamp / 1000,
+                                pos->x,
+                                pos->y,
+                                pos->z,
+                                pos->roll,
+                                pos->pitch,
+                                pos->yaw);
+        }
+    }
+};
+
+
 class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
 {
 public:
@@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+    new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };
-- 
cgit v1.2.3


From e6542653b9d013d5cb1b1c0f01ee9af7de4abe5b Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:26:15 +0200
Subject: Finished adding a '-w' option.

---
 src/modules/mavlink/mavlink_main.cpp     | 37 +++++++++++++++++++++-----------
 src/modules/mavlink/mavlink_main.h       | 12 +++++++++++
 src/modules/mavlink/mavlink_receiver.cpp |  4 ++++
 3 files changed, 40 insertions(+), 13 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1ed3f4001..b9e0663b7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	int buf_free = 0;
 
 	if (instance->get_flow_control_enabled()
-	    && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
 		if (buf_free == 0) {
 
 			if (last_write_times[(unsigned)channel] != 0 &&
-			    hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
+				hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
 
 				warnx("DISABLING HARDWARE FLOW CONTROL");
 				instance->enable_flow_control(false);
@@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		}
 	}
 
-	ssize_t ret = write(uart, ch, desired);
-
-	if (ret != desired) {
-		// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (instance->should_transmit()) {
+	   ssize_t ret = write(uart, ch, desired);
+		if (ret != desired) {
+			// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+		}
 	}
 
+
+
 }
 
 static void usage(void);
@@ -204,6 +209,8 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_hil_enabled(false),
 	_is_usb_uart(false),
+	_wait_to_transmit(false),
+	_received_messages(false),
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
@@ -1768,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1820,6 +1827,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_verbose = true;
 			break;
 
+		case 'w':
+			_wait_to_transmit = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -2164,11 +2175,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-		       SCHED_DEFAULT,
-		       SCHED_PRIORITY_DEFAULT,
-		       2048,
-		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   2048,
+			   (main_t)&Mavlink::start_helper,
+			   (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
@@ -2264,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 9941a5f99..2c1826139 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -202,6 +202,14 @@ public:
 
 	int get_mavlink_fd() { return _mavlink_fd; }
 
+
+    /* Functions for waiting to start transmission until message received. */
+    void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+    bool get_has_received_messages() { return _received_messages; }
+    void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+    bool get_wait_to_transmit() { return _wait_to_transmit; }
+    bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
 protected:
 	Mavlink	*next;
 
@@ -216,6 +224,8 @@ private:
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
 	bool		_is_usb_uart;		/**< Port is USB */
+    bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
+    bool        _received_messages; /**< Whether we've received valid mavlink messages. */
 
 	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
 
@@ -270,6 +280,8 @@ private:
 
 	pthread_mutex_t _message_buffer_mutex;
 
+
+
 	/**
 	 * Send one parameter.
 	 *
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c66350f5b..61ef2b043 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			break;
 		}
 	}
+
+    /* If we've received a valid message, mark the flag indicating so.
+       This is used in the '-w' command-line flag. */
+    _mavlink->set_has_received_messages(true);
 }
 
 void
-- 
cgit v1.2.3


From 4f91fdb98ca03f27ea130a53636b4b7990fba183 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:30:00 +0200
Subject: Indentation.

---
 src/modules/mavlink/mavlink_main.h | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 2c1826139..66d82b471 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -203,12 +203,12 @@ public:
 	int get_mavlink_fd() { return _mavlink_fd; }
 
 
-    /* Functions for waiting to start transmission until message received. */
-    void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
-    bool get_has_received_messages() { return _received_messages; }
-    void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
-    bool get_wait_to_transmit() { return _wait_to_transmit; }
-    bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+	/* Functions for waiting to start transmission until message received. */
+	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	bool get_has_received_messages() { return _received_messages; }
+	void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	bool get_wait_to_transmit() { return _wait_to_transmit; }
+	bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
 protected:
 	Mavlink	*next;
@@ -224,8 +224,8 @@ private:
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
 	bool		_is_usb_uart;		/**< Port is USB */
-    bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
-    bool        _received_messages; /**< Whether we've received valid mavlink messages. */
+	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
+	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
 
 	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
 
-- 
cgit v1.2.3


From dd88e319ee774ac9c8b7726d2fb4dbcc5b04078b Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:33:10 +0200
Subject: Reverted logging.

---
 ROMFS/px4fmu_common/init.d/rc.logging |  2 +-
 src/modules/mavlink/mavlink_main.cpp  | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

(limited to 'src')

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index d6fbe8c4f..c5aef8273 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if hw_ver compare PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 1 -t -e
+		sdlog2 start -r 50 -a -b 8 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b9e0663b7..0186cddea 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&Mavlink::start_helper,
-			   (const char **)argv);
+				   SCHED_DEFAULT,
+				   SCHED_PRIORITY_DEFAULT,
+				   2048,
+				   (main_t)&Mavlink::start_helper,
+				   (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
-- 
cgit v1.2.3


From e5105f6d9124dd12a86bec445a8b6f483adfbc56 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:35:30 +0200
Subject: Whtespace.

---
 src/modules/mavlink/mavlink_main.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 0186cddea..f4a4ce86c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-				   SCHED_DEFAULT,
-				   SCHED_PRIORITY_DEFAULT,
-				   2048,
-				   (main_t)&Mavlink::start_helper,
-				   (const char **)argv);
+		       SCHED_DEFAULT,
+		       SCHED_PRIORITY_DEFAULT,
+		       2048,
+		       (main_t)&Mavlink::start_helper,
+		       (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
-- 
cgit v1.2.3


From 8a946f0320c4bd4a61927a12b7ba4c0c96c77d7d Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:37:58 +0200
Subject: More whitespace all the time.

---
 src/modules/mavlink/mavlink_messages.cpp | 180 +++++++++++++++----------------
 src/modules/mavlink/mavlink_receiver.cpp |   6 +-
 2 files changed, 93 insertions(+), 93 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 37929edac..2d1d92243 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m)
 }
 
 void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
-			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+				uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	*mavlink_state = 0;
 	*mavlink_base_mode = 0;
@@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* arming state */
 	if (status->arming_state == ARMING_STATE_ARMED
-	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* set system state */
 	if (status->arming_state == ARMING_STATE_INIT
-	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
-	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+		|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
 		*mavlink_state = MAV_STATE_UNINIT;
 
 	} else if (status->arming_state == ARMING_STATE_ARMED) {
@@ -344,13 +344,13 @@ protected:
 			}
 
 			mavlink_msg_highres_imu_send(_channel,
-						     sensor->timestamp,
-						     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
-						     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
-						     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
-						     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
-						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
-						     fields_updated);
+							 sensor->timestamp,
+							 sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
+							 sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
+							 sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
+							 sensor->baro_pres_mbar, sensor->differential_pressure_pa,
+							 sensor->baro_alt_meter, sensor->baro_temp_celcius,
+							 fields_updated);
 		}
 	}
 };
@@ -420,14 +420,14 @@ protected:
 	{
 		if (att_sub->update(t)) {
 			mavlink_msg_attitude_quaternion_send(_channel,
-							     att->timestamp / 1000,
-							     att->q[0],
-							     att->q[1],
-							     att->q[2],
-							     att->q[3],
-							     att->rollspeed,
-							     att->pitchspeed,
-							     att->yawspeed);
+								 att->timestamp / 1000,
+								 att->q[0],
+								 att->q[1],
+								 att->q[2],
+								 att->q[3],
+								 att->rollspeed,
+								 att->pitchspeed,
+								 att->yawspeed);
 		}
 	}
 };
@@ -534,16 +534,16 @@ protected:
 	{
 		if (gps_sub->update(t)) {
 			mavlink_msg_gps_raw_int_send(_channel,
-						     gps->timestamp_position,
-						     gps->fix_type,
-						     gps->lat,
-						     gps->lon,
-						     gps->alt,
-						     cm_uint16_from_m_float(gps->eph_m),
-						     cm_uint16_from_m_float(gps->epv_m),
-						     gps->vel_m_s * 100.0f,
-						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps->satellites_visible);
+							 gps->timestamp_position,
+							 gps->fix_type,
+							 gps->lat,
+							 gps->lon,
+							 gps->alt,
+							 cm_uint16_from_m_float(gps->eph_m),
+							 cm_uint16_from_m_float(gps->epv_m),
+							 gps->vel_m_s * 100.0f,
+							 _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+							 gps->satellites_visible);
 		}
 	}
 };
@@ -586,15 +586,15 @@ protected:
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-							     pos->timestamp / 1000,
-							     pos->lat * 1e7,
-							     pos->lon * 1e7,
-							     pos->alt * 1000.0f,
-							     (pos->alt - home->alt) * 1000.0f,
-							     pos->vel_n * 100.0f,
-							     pos->vel_e * 100.0f,
-							     pos->vel_d * 100.0f,
-							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+								 pos->timestamp / 1000,
+								 pos->lat * 1e7,
+								 pos->lon * 1e7,
+								 pos->alt * 1000.0f,
+								 (pos->alt - home->alt) * 1000.0f,
+								 pos->vel_n * 100.0f,
+								 pos->vel_e * 100.0f,
+								 pos->vel_d * 100.0f,
+								 _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -628,13 +628,13 @@ protected:
 	{
 		if (pos_sub->update(t)) {
 			mavlink_msg_local_position_ned_send(_channel,
-							    pos->timestamp / 1000,
-							    pos->x,
-							    pos->y,
-							    pos->z,
-							    pos->vx,
-							    pos->vy,
-							    pos->vz);
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->vx,
+								pos->vy,
+								pos->vz);
 		}
 	}
 };
@@ -644,40 +644,40 @@ protected:
 class MavlinkStreamViconPositionEstimate : public MavlinkStream
 {
 public:
-    const char *get_name()
-    {
-        return "VICON_POSITION_ESTIMATE";
-    }
+	const char *get_name()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
 
-    MavlinkStream *new_instance()
-    {
-        return new MavlinkStreamViconPositionEstimate();
-    }
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
 
 private:
-    MavlinkOrbSubscription *pos_sub;
-    struct vehicle_vicon_position_s *pos;
+	MavlinkOrbSubscription *pos_sub;
+	struct vehicle_vicon_position_s *pos;
 
 protected:
-    void subscribe(Mavlink *mavlink)
-    {
-        pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
-        pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
-    }
-
-    void send(const hrt_abstime t)
-    {
-        if (pos_sub->update(t)) {
-            mavlink_msg_vicon_position_estimate_send(_channel,
-                                pos->timestamp / 1000,
-                                pos->x,
-                                pos->y,
-                                pos->z,
-                                pos->roll,
-                                pos->pitch,
-                                pos->yaw);
-        }
-    }
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (pos_sub->update(t)) {
+			mavlink_msg_vicon_position_estimate_send(_channel,
+							pos->timestamp / 1000,
+							pos->x,
+							pos->y,
+							pos->z,
+							pos->roll,
+							pos->pitch,
+							pos->yaw);
+		}
+	}
 };
 
 
@@ -869,10 +869,10 @@ protected:
 				}
 
 				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);
+								  hrt_absolute_time(),
+								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+								  mavlink_base_mode,
+								  0);
 			} else {
 
 				/* fixed wing: scale all channels except throttle -1 .. 1
@@ -897,10 +897,10 @@ protected:
 				}
 
 				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);
+								  hrt_absolute_time(),
+								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+								  mavlink_base_mode,
+								  0);
 			}
 		}
 	}
@@ -1175,12 +1175,12 @@ protected:
 	{
 		if (flow_sub->update(t)) {
 			mavlink_msg_optical_flow_send(_channel,
-						      flow->timestamp,
-						      flow->sensor_id,
-						      flow->flow_raw_x, flow->flow_raw_y,
-						      flow->flow_comp_x_m, flow->flow_comp_y_m,
-						      flow->quality,
-						      flow->ground_distance_m);
+							  flow->timestamp,
+							  flow->sensor_id,
+							  flow->flow_raw_x, flow->flow_raw_y,
+							  flow->flow_comp_x_m, flow->flow_comp_y_m,
+							  flow->quality,
+							  flow->ground_distance_m);
 		}
 	}
 };
@@ -1300,7 +1300,7 @@ protected:
 		(void)status_sub->update(t);
 
 		if (status->arming_state == ARMING_STATE_ARMED
-		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
@@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
-    new MavlinkStreamViconPositionEstimate(),
+	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 61ef2b043..b4fe65fd2 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -182,9 +182,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}
 	}
 
-    /* If we've received a valid message, mark the flag indicating so.
-       This is used in the '-w' command-line flag. */
-    _mavlink->set_has_received_messages(true);
+	/* If we've received a valid message, mark the flag indicating so.
+	   This is used in the '-w' command-line flag. */
+	_mavlink->set_has_received_messages(true);
 }
 
 void
-- 
cgit v1.2.3


From e7c8fdc586e59d50579470336157feb14c65ac5b Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:41:00 +0200
Subject: More whitespace all the time.

---
 src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++++++----------------
 1 file changed, 67 insertions(+), 67 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2d1d92243..648228e3b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m)
 }
 
 void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
-				uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	*mavlink_state = 0;
 	*mavlink_base_mode = 0;
@@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* arming state */
 	if (status->arming_state == ARMING_STATE_ARMED
-		|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* set system state */
 	if (status->arming_state == ARMING_STATE_INIT
-		|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
-		|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
 		*mavlink_state = MAV_STATE_UNINIT;
 
 	} else if (status->arming_state == ARMING_STATE_ARMED) {
@@ -344,13 +344,13 @@ protected:
 			}
 
 			mavlink_msg_highres_imu_send(_channel,
-							 sensor->timestamp,
-							 sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
-							 sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
-							 sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
-							 sensor->baro_pres_mbar, sensor->differential_pressure_pa,
-							 sensor->baro_alt_meter, sensor->baro_temp_celcius,
-							 fields_updated);
+						     sensor->timestamp,
+						     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
+						     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
+						     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
+						     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
+						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
+						     fields_updated);
 		}
 	}
 };
@@ -420,14 +420,14 @@ protected:
 	{
 		if (att_sub->update(t)) {
 			mavlink_msg_attitude_quaternion_send(_channel,
-								 att->timestamp / 1000,
-								 att->q[0],
-								 att->q[1],
-								 att->q[2],
-								 att->q[3],
-								 att->rollspeed,
-								 att->pitchspeed,
-								 att->yawspeed);
+							     att->timestamp / 1000,
+							     att->q[0],
+							     att->q[1],
+							     att->q[2],
+							     att->q[3],
+							     att->rollspeed,
+							     att->pitchspeed,
+							     att->yawspeed);
 		}
 	}
 };
@@ -534,16 +534,16 @@ protected:
 	{
 		if (gps_sub->update(t)) {
 			mavlink_msg_gps_raw_int_send(_channel,
-							 gps->timestamp_position,
-							 gps->fix_type,
-							 gps->lat,
-							 gps->lon,
-							 gps->alt,
-							 cm_uint16_from_m_float(gps->eph_m),
-							 cm_uint16_from_m_float(gps->epv_m),
-							 gps->vel_m_s * 100.0f,
-							 _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-							 gps->satellites_visible);
+						     gps->timestamp_position,
+						     gps->fix_type,
+						     gps->lat,
+						     gps->lon,
+						     gps->alt,
+						     cm_uint16_from_m_float(gps->eph_m),
+						     cm_uint16_from_m_float(gps->epv_m),
+						     gps->vel_m_s * 100.0f,
+						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+						     gps->satellites_visible);
 		}
 	}
 };
@@ -586,15 +586,15 @@ protected:
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-								 pos->timestamp / 1000,
-								 pos->lat * 1e7,
-								 pos->lon * 1e7,
-								 pos->alt * 1000.0f,
-								 (pos->alt - home->alt) * 1000.0f,
-								 pos->vel_n * 100.0f,
-								 pos->vel_e * 100.0f,
-								 pos->vel_d * 100.0f,
-								 _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+							     pos->timestamp / 1000,
+							     pos->lat * 1e7,
+							     pos->lon * 1e7,
+							     pos->alt * 1000.0f,
+							     (pos->alt - home->alt) * 1000.0f,
+							     pos->vel_n * 100.0f,
+							     pos->vel_e * 100.0f,
+							     pos->vel_d * 100.0f,
+							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -628,13 +628,13 @@ protected:
 	{
 		if (pos_sub->update(t)) {
 			mavlink_msg_local_position_ned_send(_channel,
-								pos->timestamp / 1000,
-								pos->x,
-								pos->y,
-								pos->z,
-								pos->vx,
-								pos->vy,
-								pos->vz);
+							    pos->timestamp / 1000,
+							    pos->x,
+							    pos->y,
+							    pos->z,
+							    pos->vx,
+							    pos->vy,
+							    pos->vz);
 		}
 	}
 };
@@ -669,13 +669,13 @@ protected:
 	{
 		if (pos_sub->update(t)) {
 			mavlink_msg_vicon_position_estimate_send(_channel,
-							pos->timestamp / 1000,
-							pos->x,
-							pos->y,
-							pos->z,
-							pos->roll,
-							pos->pitch,
-							pos->yaw);
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->roll,
+								pos->pitch,
+								pos->yaw);
 		}
 	}
 };
@@ -869,10 +869,10 @@ protected:
 				}
 
 				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);
+							      hrt_absolute_time(),
+							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+							      mavlink_base_mode,
+							      0);
 			} else {
 
 				/* fixed wing: scale all channels except throttle -1 .. 1
@@ -897,10 +897,10 @@ protected:
 				}
 
 				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);
+							      hrt_absolute_time(),
+							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+							      mavlink_base_mode,
+							      0);
 			}
 		}
 	}
@@ -1175,12 +1175,12 @@ protected:
 	{
 		if (flow_sub->update(t)) {
 			mavlink_msg_optical_flow_send(_channel,
-							  flow->timestamp,
-							  flow->sensor_id,
-							  flow->flow_raw_x, flow->flow_raw_y,
-							  flow->flow_comp_x_m, flow->flow_comp_y_m,
-							  flow->quality,
-							  flow->ground_distance_m);
+						      flow->timestamp,
+						      flow->sensor_id,
+						      flow->flow_raw_x, flow->flow_raw_y,
+						      flow->flow_comp_x_m, flow->flow_comp_y_m,
+						      flow->quality,
+						      flow->ground_distance_m);
 		}
 	}
 };
@@ -1300,7 +1300,7 @@ protected:
 		(void)status_sub->update(t);
 
 		if (status->arming_state == ARMING_STATE_ARMED
-			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
-- 
cgit v1.2.3


From 88357c58bde304567f841abc48a495794d4e250c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Apr 2014 20:45:03 +0200
Subject: mavlink: Change to size optimization

---
 src/modules/mavlink/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 515fbfadc..c44354ff0 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -46,3 +46,5 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_commands.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION		 = -Os
-- 
cgit v1.2.3