Skip to content

Commit

Permalink
Info fixups (#2284)
Browse files Browse the repository at this point in the history
* camera/core: remove some Yuneec baggage

I don't think we need to keep maintaining old Yuneec stuff.

Signed-off-by: Julian Oes <[email protected]>

* info: fix received flags

I mixed this up.

Signed-off-by: Julian Oes <[email protected]>

---------

Signed-off-by: Julian Oes <[email protected]>
  • Loading branch information
julianoes authored Apr 16, 2024
1 parent f074901 commit 31f80f6
Show file tree
Hide file tree
Showing 6 changed files with 19 additions and 108 deletions.
27 changes: 0 additions & 27 deletions src/mavsdk/core/system_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -526,33 +526,6 @@ void SystemImpl::send_autopilot_version_request_async(
send_command_async(command, callback);
}

void SystemImpl::send_flight_information_request()
{
auto prom = std::promise<MavlinkCommandSender::Result>();
auto fut = prom.get_future();

MavlinkCommandSender::CommandLong command{};
command.target_component_id = get_autopilot_id();

if (_old_message_528_supported) {
// Note: This MAVLINK message is deprecated and would be removed from MAVSDK in a future
// release.
command.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
command.params.maybe_param1 = 1.0f;
} else {
command.command = MAV_CMD_REQUEST_MESSAGE;
command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_FLIGHT_INFORMATION)};
}

send_command_async(
command, [&prom](MavlinkCommandSender::Result result, float) { prom.set_value(result); });
if (fut.get() == MavlinkCommandSender::Result::Unsupported) {
_old_message_528_supported = false;
LogWarn() << "Trying alternative command (512)..";
send_flight_information_request();
}
}

void SystemImpl::set_connected()
{
bool enable_needed = false;
Expand Down
1 change: 0 additions & 1 deletion src/mavsdk/core/system_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -283,7 +283,6 @@ class SystemImpl {
void send_autopilot_version_request();
void send_autopilot_version_request_async(
const MavlinkCommandSender::CommandResultCallback& callback);
void send_flight_information_request();

MavlinkMissionTransferClient& mission_transfer_client() { return _mission_transfer_client; };

Expand Down
72 changes: 0 additions & 72 deletions src/mavsdk/plugins/camera/camera_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,13 +79,6 @@ void CameraImpl::init()
[this](const mavlink_message_t& message) { process_video_stream_status(message); },
this);

if (_system_impl->has_autopilot()) {
_system_impl->register_mavlink_message_handler(
MAVLINK_MSG_ID_FLIGHT_INFORMATION,
[this](const mavlink_message_t& message) { process_flight_information(message); },
this);
}

_system_impl->add_call_every(
[this]() { check_connection_status(); }, 0.5, &_check_connection_status_call_every_cookie);

Expand All @@ -99,7 +92,6 @@ void CameraImpl::deinit()
_system_impl->remove_call_every(_check_connection_status_call_every_cookie);
_system_impl->remove_call_every(_status.call_every_cookie);
_system_impl->remove_call_every(_camera_information_call_every_cookie);
_system_impl->remove_call_every(_flight_information_call_every_cookie);
_system_impl->remove_call_every(_mode.call_every_cookie);
_system_impl->remove_call_every(_video_stream_info.call_every_cookie);
_system_impl->unregister_all_mavlink_message_handlers(this);
Expand Down Expand Up @@ -207,16 +199,6 @@ void CameraImpl::manual_enable()
[this]() { request_camera_information(); }, 10.0, &_camera_information_call_every_cookie);

_system_impl->add_call_every([this]() { request_status(); }, 5.0, &_status.call_every_cookie);

// for backwards compatibility with Yuneec drones
if (_system_impl->has_autopilot()) {
request_flight_information();

_system_impl->add_call_every(
[this]() { request_flight_information(); },
10.0,
&_flight_information_call_every_cookie);
}
}

void CameraImpl::disable()
Expand All @@ -232,10 +214,6 @@ void CameraImpl::manual_disable()
_system_impl->remove_call_every(_camera_information_call_every_cookie);
_system_impl->remove_call_every(_status.call_every_cookie);

if (_flight_information_call_every_cookie) {
_system_impl->remove_call_every(_flight_information_call_every_cookie);
}

_camera_found = false;
}

Expand Down Expand Up @@ -284,17 +262,6 @@ Camera::Result CameraImpl::select_camera(const size_t id)
return Camera::Result::Success;
}

MavlinkCommandSender::CommandLong CameraImpl::make_command_request_flight_information()
{
MavlinkCommandSender::CommandLong command_flight_information{};

command_flight_information.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
command_flight_information.params.maybe_param1 = 1.0f; // Request it
command_flight_information.target_component_id = MAV_COMP_ID_AUTOPILOT1;

return command_flight_information;
}

MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_info()
{
MavlinkCommandSender::CommandLong command_camera_info{};
Expand Down Expand Up @@ -1678,39 +1645,6 @@ void CameraImpl::process_video_stream_status(const mavlink_message_t& message)
notify_video_stream_info();
}

void CameraImpl::process_flight_information(const mavlink_message_t& message)
{
mavlink_flight_information_t flight_information;
mavlink_msg_flight_information_decode(&message, &flight_information);

std::stringstream folder_name_stream;
{
std::lock_guard<std::mutex> information_lock(_information.mutex);

// For Yuneec cameras, the folder names can be derived from the flight ID,
// starting at 100 up to 999.
if (_information.data.vendor_name == "Yuneec" && _information.data.model_name == "E90") {
folder_name_stream << (101 + flight_information.flight_uuid % 899) << "E90HD";
} else if (
_information.data.vendor_name == "Yuneec" && _information.data.model_name == "E50") {
folder_name_stream << (101 + flight_information.flight_uuid % 899) << "E50HD";
} else if (
_information.data.vendor_name == "Yuneec" && _information.data.model_name == "CGOET") {
folder_name_stream << (101 + flight_information.flight_uuid % 899) << "CGOET";
} else if (
_information.data.vendor_name == "Yuneec" && _information.data.model_name == "E10T") {
folder_name_stream << (101 + flight_information.flight_uuid % 899) << "E10T";
} else {
// Folder name unknown
}
}

{
std::lock_guard<std::mutex> lock(_status.mutex);
_status.data.media_folder_name = folder_name_stream.str();
}
}

void CameraImpl::notify_video_stream_info()
{
std::lock_guard<std::mutex> lock(_video_stream_info.mutex);
Expand Down Expand Up @@ -2283,12 +2217,6 @@ void CameraImpl::request_camera_settings()
_system_impl->send_command_async(command_camera_settings, nullptr);
}

void CameraImpl::request_flight_information()
{
auto command_flight_information = make_command_request_flight_information();
_system_impl->send_command_async(command_flight_information, nullptr);
}

void CameraImpl::request_camera_information()
{
auto command_camera_info = make_command_request_camera_info();
Expand Down
4 changes: 0 additions & 4 deletions src/mavsdk/plugins/camera/camera_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,6 @@ class CameraImpl : public PluginImplBase {
void process_camera_information(const mavlink_message_t& message);
void process_video_information(const mavlink_message_t& message);
void process_video_stream_status(const mavlink_message_t& message);
void process_flight_information(const mavlink_message_t& message);
void reset_following_format_storage();

Camera::Status::StorageStatus storage_status_from_mavlink(const int storage_status) const;
Expand Down Expand Up @@ -205,7 +204,6 @@ class CameraImpl : public PluginImplBase {
Camera::Mode to_camera_mode(const uint8_t mavlink_camera_mode) const;

void* _camera_information_call_every_cookie{nullptr};
void* _flight_information_call_every_cookie{nullptr};
void* _check_connection_status_call_every_cookie{nullptr};
void* _request_missing_capture_info_cookie{nullptr};

Expand All @@ -214,12 +212,10 @@ class CameraImpl : public PluginImplBase {
void request_video_stream_info();
void request_video_stream_status();
void request_status();
void request_flight_information();

MavlinkCommandSender::CommandLong make_command_take_photo(float interval_s, float no_of_photos);
MavlinkCommandSender::CommandLong make_command_stop_photo();

MavlinkCommandSender::CommandLong make_command_request_flight_information();
MavlinkCommandSender::CommandLong make_command_request_camera_info();
MavlinkCommandSender::CommandLong make_command_set_camera_mode(float mavlink_mode);
MavlinkCommandSender::CommandLong make_command_request_camera_settings();
Expand Down
22 changes: 18 additions & 4 deletions src/mavsdk/plugins/info/info_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@ std::string InfoImpl::translate_binary_to_str(uint8_t* binary, unsigned binary_l

std::pair<Info::Result, Info::Identification> InfoImpl::get_identification() const
{
wait_for_information();
wait_for_identification();

std::lock_guard<std::mutex> lock(_mutex);
return std::make_pair<>(
Expand All @@ -199,7 +199,7 @@ std::pair<Info::Result, Info::Identification> InfoImpl::get_identification() con

std::pair<Info::Result, Info::Version> InfoImpl::get_version() const
{
wait_for_information();
wait_for_identification();

std::lock_guard<std::mutex> lock(_mutex);

Expand All @@ -211,7 +211,7 @@ std::pair<Info::Result, Info::Version> InfoImpl::get_version() const

std::pair<Info::Result, Info::Product> InfoImpl::get_product() const
{
wait_for_information();
wait_for_identification();
std::lock_guard<std::mutex> lock(_mutex);

return std::make_pair<>(
Expand Down Expand Up @@ -290,7 +290,7 @@ std::pair<Info::Result, double> InfoImpl::get_speed_factor() const
return std::make_pair<>(Info::Result::Success, speed_factor);
}

void InfoImpl::wait_for_information() const
void InfoImpl::wait_for_identification() const
{
// Wait 1.5 seconds max
for (unsigned i = 0; i < 150; ++i) {
Expand All @@ -304,4 +304,18 @@ void InfoImpl::wait_for_information() const
}
}

void InfoImpl::wait_for_information() const
{
// Wait 1.5 seconds max
for (unsigned i = 0; i < 150; ++i) {
{
std::lock_guard<std::mutex> lock(_mutex);
if (_flight_information_received) {
break;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}

} // namespace mavsdk
1 change: 1 addition & 0 deletions src/mavsdk/plugins/info/info_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ class InfoImpl : public PluginImplBase {
Info::Version::FlightSoftwareVersionType
get_flight_software_version_type(FIRMWARE_VERSION_TYPE);

void wait_for_identification() const;
void wait_for_information() const;

mutable std::mutex _mutex{};
Expand Down

0 comments on commit 31f80f6

Please sign in to comment.