Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal

3 files changed, 110 insertions, 71 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 558be4275..2af790fc4 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -6,6 +6,10 @@
 echo "Starting MAVLink on this USB console"
 
 mavlink start -r 10000 -d /dev/ttyACM0
+# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30
 
 # Exit shell to make it available to MAVLink
 exit 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c9f4835ba..00d906dcd 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -588,7 +588,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 		/* setup output flow control */
 		if (enable_flow_control(true)) {
-			warnx("ERR FLOW CTRL EN");
+			warnx("hardware flow control not supported");
 		}
 	}
 
@@ -1762,6 +1762,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
 		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);
 		break;
 
 	default:
@@ -1951,8 +1952,17 @@ Mavlink::start(int argc, char *argv[])
 	// the only path to create a new instance,
 	// this is effectively a lock on concurrent
 	// instance starting. XXX do a real lock.
-	while (ic == Mavlink::instance_count()) {
-		::usleep(500);
+
+	// Sleep 500 us between each attempt
+	const unsigned sleeptime = 500;
+
+	// Wait 100 ms max for the startup.
+	const unsigned limit = 100 * 1000 / sleeptime;
+
+	unsigned count = 0;
+	while (ic == Mavlink::instance_count() && count < limit) {
+		::usleep(sleeptime);
+		count++;
 	}
 
 	return OK;
@@ -2008,7 +2018,10 @@ Mavlink::stream(int argc, char *argv[])
 			inst->configure_stream_threadsafe(stream_name, rate);
 
 		} else {
-			errx(1, "mavlink for device %s is not running", device_name);
+
+			// If the link is not running we should complain, but not fall over
+			// because this is so easy to get wrong and not fatal. Warning is sufficient.
+			errx(0, "mavlink for device %s is not running", device_name);
 		}
 
 	} else {
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7d388f88d..dc01935ca 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1127,6 +1127,93 @@ protected:
 	}
 };
 
+class MavlinkStreamAttitudeControls : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "ATTITUDE_CONTROLS";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeControls();
+	}
+
+private:
+	MavlinkOrbSubscription *att_ctrl_sub;
+	struct actuator_controls_s *att_ctrl;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+		att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (att_ctrl_sub->update(t)) {
+
+		// send, add spaces so that string buffer is at least 10 chars long
+		mavlink_msg_named_value_float_send(_channel,
+						   att_ctrl->timestamp / 1000,
+						   "rll ctrl    ",
+						   att_ctrl->control[0]);
+		mavlink_msg_named_value_float_send(_channel,
+						   att_ctrl->timestamp / 1000,
+						   "ptch ctrl    ",
+						   att_ctrl->control[1]);
+		mavlink_msg_named_value_float_send(_channel,
+						   att_ctrl->timestamp / 1000,
+						   "yaw ctrl     ",
+						   att_ctrl->control[2]);
+		mavlink_msg_named_value_float_send(_channel,
+						   att_ctrl->timestamp / 1000,
+						   "thr ctrl     ",
+						   att_ctrl->control[3]);
+		}
+	}
+};
+
+class MavlinkStreamNamedValueFloat : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "NAMED_VALUE_FLOAT";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamNamedValueFloat();
+	}
+
+private:
+	MavlinkOrbSubscription *debug_sub;
+	struct debug_key_value_s *debug;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
+		debug = (struct debug_key_value_s *)debug_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (debug_sub->update(t)) {
+
+			// Enforce null termination
+			debug->key[sizeof(debug->key) - 1] = '\0';
+
+			mavlink_msg_named_value_float_send(_channel,
+							   debug->timestamp_ms,
+							   debug->key,
+							   debug->value);
+		}
+	}
+};
 
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
@@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamRCChannelsRaw(),
 	new MavlinkStreamManualControl(),
 	new MavlinkStreamOpticalFlow(),
+	new MavlinkStreamAttitudeControls(),
+	new MavlinkStreamNamedValueFloat(),
 	nullptr
 };
-
-
-
-
-
-
-
-//
-//
-//
-//
-//
-//
-//void
-//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
-//{
-//	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
-//
-//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-//		/* send, add spaces so that string buffer is at least 10 chars long */
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl0    ",
-//						   l->listener->actuators_0.control[0]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl1    ",
-//						   l->listener->actuators_0.control[1]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl2     ",
-//						   l->listener->actuators_0.control[2]);
-//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//						   l->listener->last_sensor_timestamp / 1000,
-//						   "ctrl3     ",
-//						   l->listener->actuators_0.control[3]);
-//	}
-//}
-//
-//void
-//MavlinkOrbListener::l_debug_key_value(const struct listener *l)
-//{
-//	struct debug_key_value_s debug;
-//
-//	orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
-//
-//	/* Enforce null termination */
-//	debug.key[sizeof(debug.key) - 1] = '\0';
-//
-//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//					   l->listener->last_sensor_timestamp / 1000,
-//					   debug.key,
-//					   debug.value);
-//}
-//
-//void
-//MavlinkOrbListener::l_nav_cap(const struct listener *l)
-//{
-//
-//	orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
-//
-//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-//				   hrt_absolute_time() / 1000,
-//				   "turn dist",
-//				   l->listener->nav_cap.turn_distance);
-//
-//}