implemented MAV_CMD_REQUEST_MESSAGE for STORAGE_INFORMATION

src/modules/mavlink/mavlink_messages.cpp

@@ -5172,6 +5172,103 @@ protected:
 	}
 };
 
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+
+class MavlinkStreamStorageInformation : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamStorageInformation::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "STORAGE_INFORMATION";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_STORAGE_INFORMATION;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamStorageInformation(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	virtual bool request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
+				     float param5 = 0.0, float param6 = 0.0, float param7 = 0.0)
+	{
+		storage_id = (int)roundf(param2);
+		return send(hrt_absolute_time());
+	}
+private:
+	int storage_id = 0;
+
+	/* do not allow top copying this class */
+	MavlinkStreamStorageInformation(MavlinkStreamStorageInformation &) = delete;
+	MavlinkStreamStorageInformation &operator = (const MavlinkStreamStorageInformation &) = delete;
+
+
+protected:
+	explicit MavlinkStreamStorageInformation(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		mavlink_storage_information_t storage_info{};
+		const char *microsd_dir = PX4_STORAGEDIR;
+
+		if (storage_id == 0 || storage_id == 1) { // request is for all or the first storage
+			storage_info.storage_id = storage_id; // replace by 1
+
+			struct statfs statfs_buf;
+			uint64_t total_bytes = 0;
+			uint64_t avail_bytes = 0;
+
+			if (statfs(microsd_dir, &statfs_buf) == 0) {
+				total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
+				avail_bytes = (uint64_t)statfs_buf.f_bavail * statfs_buf.f_bsize;
+			}
+
+			if (total_bytes == 0) { // on NuttX we get 0 total bytes if no SD card is inserted
+				storage_info.storage_count = 0;
+				storage_info.status = 0; // not available
+
+			} else {
+				storage_info.storage_count = 1;
+				storage_info.status = 2; // available & formatted
+				storage_info.total_capacity = total_bytes / 1024. / 1024.;
+				storage_info.available_capacity = avail_bytes / 1024. / 1024.;
+				storage_info.used_capacity = (total_bytes - avail_bytes) / 1024. / 1024.;
+			}
+
+		} else {
+			return false; // results in MAV_RESULT_DENIED
+		}
+
+		storage_info.time_boot_ms = t / 1000;
+		mavlink_msg_storage_information_send_struct(_mavlink->get_channel(), &storage_info);
+		return true;
+	}
+};
+
 static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamHeartbeat>(),
 	create_stream_list_item<MavlinkStreamStatustext>(),
@@ -5231,7 +5328,8 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamGroundTruth>(),
 	create_stream_list_item<MavlinkStreamPing>(),
 	create_stream_list_item<MavlinkStreamOrbitStatus>(),
-	create_stream_list_item<MavlinkStreamObstacleDistance>()
+	create_stream_list_item<MavlinkStreamObstacleDistance>(),
+	create_stream_list_item<MavlinkStreamStorageInformation>()
 };
 
 const char *get_stream_name(const uint16_t msg_id)
@@ -5259,3 +5357,15 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink)
 
 	return nullptr;
 }
+
+MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink)
+{
+	// search for stream with specified name in supported streams list
+	for (const auto &stream : streams_list) {
+		if (msg_id == stream.get_id()) {
+			return stream.new_instance(mavlink);
+		}
+	}
+
+	return nullptr;
+}

src/modules/mavlink/mavlink_messages.h

@@ -68,6 +68,7 @@ static StreamListItem create_stream_list_item()
 
 const char *get_stream_name(const uint16_t msg_id);
 MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink);
+MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink);
 
 void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode,
 				 union px4_custom_mode *custom_mode);

src/modules/mavlink/mavlink_receiver.cpp

@@ -57,13 +57,6 @@
 #include <netinet/in.h>
 #endif
 
-#ifdef __PX4_DARWIN
-#include <sys/param.h>
-#include <sys/mount.h>
-#else
-#include <sys/statfs.h>
-#endif
-
 #ifndef __PX4_POSIX
 #include <termios.h>
 #endif
@@ -337,6 +330,7 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c
 
 	switch (command) {
 
+	case MAV_CMD_REQUEST_MESSAGE:
 	case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
 	case MAV_CMD_REQUEST_PROTOCOL_VERSION:
 		/* broadcast and ignore component */
@@ -369,46 +363,6 @@ MavlinkReceiver::send_flight_information()
 	mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info);
 }
 
-void
-MavlinkReceiver::send_storage_information(int storage_id)
-{
-	mavlink_storage_information_t storage_info{};
-	const char *microsd_dir = PX4_STORAGEDIR;
-
-	if (storage_id == 0 || storage_id == 1) { // request is for all or the first storage
-		storage_info.storage_id = 1;
-
-		struct statfs statfs_buf;
-		uint64_t total_bytes = 0;
-		uint64_t avail_bytes = 0;
-
-		if (statfs(microsd_dir, &statfs_buf) == 0) {
-			total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
-			avail_bytes = (uint64_t)statfs_buf.f_bavail * statfs_buf.f_bsize;
-		}
-
-		if (total_bytes == 0) { // on NuttX we get 0 total bytes if no SD card is inserted
-			storage_info.storage_count = 0;
-			storage_info.status = 0; // not available
-
-		} else {
-			storage_info.storage_count = 1;
-			storage_info.status = 2; // available & formatted
-			storage_info.total_capacity = total_bytes / 1024. / 1024.;
-			storage_info.available_capacity = avail_bytes / 1024. / 1024.;
-			storage_info.used_capacity = (total_bytes - avail_bytes) / 1024. / 1024.;
-		}
-
-	} else {
-		// only one storage supported
-		storage_info.storage_id = storage_id;
-		storage_info.storage_count = 1;
-	}
-
-	storage_info.time_boot_ms = hrt_absolute_time() / 1000;
-	mavlink_msg_storage_information_send_struct(_mavlink->get_channel(), &storage_info);
-}
-
 void
 MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 {
@@ -503,27 +457,34 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
 		send_flight_information();
 
-	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) {
-		if ((int)roundf(cmd_mavlink.param1) == 1) {
-			send_storage_information((int)roundf(cmd_mavlink.param1));
-		}
-
 	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
 		uint16_t message_id = (uint16_t)roundf(cmd_mavlink.param1);
 		bool stream_found = false;
 
-		for (const auto &stream : _mavlink->get_streams()) {
+		for (const auto stream : _mavlink->get_streams()) {
 			if (stream->get_id() == message_id) {
-				stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
+				stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
-							vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
+								       vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
-				stream_found = true;
 				break;
 			}
 		}
 
-		// TODO: Handle the case where a message is requested which could be sent, but for which there is no stream.
 		if (!stream_found) {
-			result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+			MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink);
+
+			if (stream == nullptr) {
+				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+
+			} else {
+				bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
+						    vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
+
+				if (!message_sent) {
+					result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+				}
+
+				delete stream;
+			}
 		}
 
 	} else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) {

src/modules/mavlink/mavlink_stream.h

@@ -96,8 +96,12 @@ public:
 	 * This function is called in response to a MAV_CMD_REQUEST_MESSAGE command. The default implementation is to
 	 * just reset the counter to immediately send one message.
 	 */
-	virtual void request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
+	virtual bool request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
-				     float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) { reset_last_sent(); }
+				     float param5 = 0.0, float param6 = 0.0, float param7 = 0.0)
+	{
+		reset_last_sent();
+		return true;
+	}
 
 	/**
 	 * Get the average message size