diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1180a38394..2be212c583 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5172,6 +5172,103 @@ protected: } }; +#ifdef __PX4_DARWIN +#include +#include +#else +#include +#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(), create_stream_list_item(), @@ -5231,7 +5328,8 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), - create_stream_list_item() + create_stream_list_item(), + create_stream_list_item() }; 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; +} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 2c775329ca..153b529848 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cec9929f6f..9a6ca1738e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -57,13 +57,6 @@ #include #endif -#ifdef __PX4_DARWIN -#include -#include -#else -#include -#endif - #ifndef __PX4_POSIX #include #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) { diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index fe1ac84dc7..91708aa693 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/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, - 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