added AUTOPILOT_VERSION and PROTOCOL_VERSION streams

src/modules/mavlink/mavlink_main.cpp

@@ -1143,8 +1143,8 @@ Mavlink::send_statustext_emergency(const char *string)
 	mavlink_log_emergency(&_mavlink_log_pub, "%s", string);
 }
 
-void
-Mavlink::send_autopilot_capabilites()
+bool
+Mavlink::send_autopilot_capabilities()
 {
 	uORB::Subscription status_sub{ORB_ID(vehicle_status)};
 	vehicle_status_s status;
@@ -1207,7 +1207,10 @@ Mavlink::send_autopilot_capabilites()
 		msg.uid2[0] += mavlink_system.sysid - 1;
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
 		mavlink_msg_autopilot_version_send_struct(get_channel(), &msg);
+		return true;
 	}
+
+	return false;
 }
 
 void
@@ -2203,7 +2206,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* if the protocol is serial, we send the system version blindly */
 	if (get_protocol() == Protocol::SERIAL) {
-		send_autopilot_capabilites();
+		send_autopilot_capabilities();
 	}
 
 	/* start the MAVLink receiver last to avoid a race */

src/modules/mavlink/mavlink_main.h

@@ -387,7 +387,7 @@ public:
 	/**
 	 * Send the capabilities of this autopilot in terms of the MAVLink spec
 	 */
-	void 			send_autopilot_capabilites();
+	bool 			send_autopilot_capabilities();
 
 	/**
 	 * Send the protocol version of MAVLink

src/modules/mavlink/mavlink_messages.cpp

@@ -45,6 +45,8 @@
 #include "mavlink_simple_analyzer.h"
 #include "mavlink_high_latency2.h"
 
+#include "streams/autopilot_version.h"
+#include "streams/protocol_version.h"
 #include "streams/storage_information.h"
 
 #include <commander/px4_custom_mode.h>

src/modules/mavlink/mavlink_receiver.cpp

@@ -435,15 +435,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 		return;
 	}
 
-	if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
-		/* send autopilot version message */
-		_mavlink->send_autopilot_capabilites();
-
-	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
-		/* send protocol version message */
-		_mavlink->send_protocol_version();
-
-	} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
+	if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
 		_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
 
 	} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {

src/modules/mavlink/streams/autopilot_version.h

@@ -0,0 +1,60 @@
+#ifndef MAVLINK_STREAM_AUTOPILOT_VERSION_H
+#define MAVLINK_STREAM_AUTOPILOT_VERSION_H
+
+#include "../mavlink_messages.h"
+
+class MavlinkStreamAutopilotVersion : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamAutopilotVersion::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "AUTOPILOT_VERSION";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_AUTOPILOT_VERSION;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamAutopilotVersion(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	bool request_message(float param1, float param2, float param3, float param4,
+			     float param5, float param6, float param7) override
+	{
+		return send(hrt_absolute_time());
+	}
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamAutopilotVersion(MavlinkStreamAutopilotVersion &) = delete;
+	MavlinkStreamAutopilotVersion &operator = (const MavlinkStreamAutopilotVersion &) = delete;
+
+
+protected:
+	explicit MavlinkStreamAutopilotVersion(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		return _mavlink->send_autopilot_capabilities();
+	}
+};
+
+#endif

src/modules/mavlink/streams/protocol_version.h

@@ -0,0 +1,62 @@
+#ifndef MAVLINK_STREAM_PROTOCOL_VERSION_H
+#define MAVLINK_STREAM_PROTOCOL_VERSION_H
+
+#include "../mavlink_messages.h"
+
+class MavlinkStreamProtocolVersion : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamProtocolVersion::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "PROTOCOL_VERSION";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_PROTOCOL_VERSION;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamProtocolVersion(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	bool request_message(float param1, float param2, float param3, float param4,
+			     float param5, float param6, float param7) override
+	{
+		return send(hrt_absolute_time());
+	}
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamProtocolVersion(MavlinkStreamProtocolVersion &) = delete;
+	MavlinkStreamProtocolVersion &operator = (const MavlinkStreamProtocolVersion &) = delete;
+
+
+protected:
+	explicit MavlinkStreamProtocolVersion(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		_mavlink->send_protocol_version();
+		return true;
+	}
+};
+
+#endif
+

src/modules/mavlink/streams/storage_information.h

@@ -10,6 +10,8 @@
 #include <sys/statfs.h>
 #endif
 
+#include <math.h>
+
 class MavlinkStreamStorageInformation : public MavlinkStream
 {
 public: