forked from Archive/PX4-Autopilot
implemented MAV_CMD_REQUEST_MESSAGE for STORAGE_INFORMATION
This commit is contained in:
parent
25bfcf0497
commit
fd09198c7e
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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,
|
||||
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
|
||||
stream_found = true;
|
||||
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);
|
||||
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) {
|
||||
|
|
|
@ -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,
|
||||
float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) { reset_last_sent(); }
|
||||
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();
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the average message size
|
||||
|
|
Loading…
Reference in New Issue