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

1 files changed, 89 insertions, 67 deletions

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);
-//
-//}