Merged master

src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

@@ -959,7 +959,7 @@ FixedwingEstimator::task_main()
 			}
 
 			// Publish results
-			if (_initialized) {
+			if (_initialized && (check == OK)) {
 
 
 

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),
@@ -825,10 +832,10 @@ void Mavlink::publish_mission()
 {
 	/* Initialize mission publication if necessary */
 	if (_mission_pub < 0) {
-		_mission_pub = orb_advertise(ORB_ID(mission), &mission);
+		_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
 
 	} else {
-		orb_publish(ORB_ID(mission), _mission_pub, &mission);
+		orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
 	}
 }
 
@@ -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;
@@ -2265,7 +2276,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[])

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.
 	 *

src/modules/mavlink/mavlink_messages.cpp

@@ -641,6 +641,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:
@@ -1344,5 +1385,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
 	new MavlinkStreamDistanceSensor(),
+	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };

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

src/modules/navigator/navigator_main.cpp

@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 {
 	struct mission_s offboard_mission;
 
-	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
+	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
 {
 	struct mission_s onboard_mission;
 
-	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
+	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
 		_mission.set_onboard_mission_count(onboard_mission.count);
 		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +611,7 @@ Navigator::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));

src/modules/uORB/objects_common.cpp

@@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
 
 #include "topics/mission.h"
-ORB_DEFINE(mission, struct mission_s);
+ORB_DEFINE(offboard_mission, struct mission_s);
 ORB_DEFINE(onboard_mission, struct mission_s);
 
 #include "topics/mission_result.h"

src/modules/uORB/topics/mission.h

@@ -105,7 +105,7 @@ struct mission_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(mission);
+ORB_DECLARE(offboard_mission);
 ORB_DECLARE(onboard_mission);
 
 #endif