From 2fa8783795f514a0318c7dd4e281a43373b83ece Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 5 Jun 2018 10:38:00 +0200
Subject: [PATCH] mavlink_receiver: implement
 MAV_CMD_REQUEST_STORAGE_INFORMATION

---
 src/modules/mavlink/mavlink_receiver.cpp | 53 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  2 +
 2 files changed, 55 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dc6eb1c04e..793eee9286 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -71,6 +71,14 @@
 #include <stdlib.h>
 #include <poll.h>
 
+#include <sys/stat.h>
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+
 #include <mathlib/mathlib.h>
 
 #include <conversion/rotation.h>
@@ -438,6 +446,46 @@ 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_ROOTFSDIR"/fs/microsd";
+
+	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)
 {
@@ -533,6 +581,11 @@ 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)(cmd_mavlink.param2 + 0.5f) == 1) {
+			send_storage_information(cmd_mavlink.param1 + 0.5f);
+		}
+
 	} else {
 
 		send_ack = false;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 7fa65bc42f..9bd9a7cc2d 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -190,6 +190,8 @@ private:
 
 	void send_flight_information();
 
+	void send_storage_information(int storage_id);
+
 	Mavlink	*_mavlink;
 
 	MavlinkMissionManager		*_mission_manager;