fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: implement MAV_CMD_REQUEST_STORAGE_INFORMATION

Browse Source
This commit is contained in:
Beat Küng 2018-06-05 10:38:00 +02:00 committed by Lorenz Meier
parent ca5e22583f
commit 2fa8783795
2 changed files with 55 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -190,6 +190,8 @@ private:
void send_flight_information();
void send_storage_information(int storage_id);
Mavlink *_mavlink;
MavlinkMissionManager *_mission_manager;