From 3198142d20b296cc5c02b4eb93bcfb4391cc94fe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Jun 2024 14:09:34 +1200 Subject: [PATCH] core: don't use callback queue twice We already use the queue when calling the user callback, so we don't need to do it internally as well. --- src/mavsdk/core/mavlink_ftp_client.cpp | 127 ++++++++++--------------- src/mavsdk/core/mavlink_ftp_client.h | 7 -- 2 files changed, 52 insertions(+), 82 deletions(-) diff --git a/src/mavsdk/core/mavlink_ftp_client.cpp b/src/mavsdk/core/mavlink_ftp_client.cpp index fbd738a7c..157ad4cb6 100644 --- a/src/mavsdk/core/mavlink_ftp_client.cpp +++ b/src/mavsdk/core/mavlink_ftp_client.cpp @@ -167,7 +167,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->req_opcode == CMD_TERMINATE_SESSION) { stop_timer(); item.ofstream.close(); - call_callback(item.callback, ClientResult::Success, {}); + item.callback(ClientResult::Success, {}); work_queue_guard.pop_front(); } else { @@ -176,7 +176,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload), {}); + item.callback(result_from_nak(payload), {}); terminate_session(*work); work_queue_guard.pop_front(); } @@ -197,7 +197,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->req_opcode == CMD_TERMINATE_SESSION) { stop_timer(); item.ofstream.close(); - call_callback(item.callback, ClientResult::Success, {}); + item.callback(ClientResult::Success, {}); work_queue_guard.pop_front(); } else { @@ -215,7 +215,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else { LogWarn() << "FTP: NAK received"; stop_timer(); - call_callback(item.callback, result_from_nak(payload), {}); + item.callback(result_from_nak(payload), {}); terminate_session(*work); work_queue_guard.pop_front(); } @@ -237,7 +237,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->req_opcode == CMD_TERMINATE_SESSION) { stop_timer(); item.ifstream.close(); - call_callback(item.callback, ClientResult::Success, {}); + item.callback(ClientResult::Success, {}); work_queue_guard.pop_front(); } else { @@ -246,7 +246,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload), {}); + item.callback(result_from_nak(payload), {}); terminate_session(*work); work_queue_guard.pop_front(); } @@ -255,7 +255,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->opcode == RSP_ACK) { if (payload->req_opcode == CMD_REMOVE_FILE) { stop_timer(); - call_callback(item.callback, ClientResult::Success); + item.callback(ClientResult::Success); work_queue_guard.pop_front(); } else { @@ -264,7 +264,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload)); + item.callback(result_from_nak(payload)); terminate_session(*work); work_queue_guard.pop_front(); } @@ -273,7 +273,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->opcode == RSP_ACK) { if (payload->req_opcode == CMD_RENAME) { stop_timer(); - call_callback(item.callback, ClientResult::Success); + item.callback(ClientResult::Success); work_queue_guard.pop_front(); } else { @@ -282,7 +282,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload)); + item.callback(result_from_nak(payload)); terminate_session(*work); work_queue_guard.pop_front(); } @@ -291,7 +291,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->opcode == RSP_ACK) { if (payload->req_opcode == CMD_CREATE_DIRECTORY) { stop_timer(); - call_callback(item.callback, ClientResult::Success); + item.callback(ClientResult::Success); work_queue_guard.pop_front(); } else { @@ -300,7 +300,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload)); + item.callback(result_from_nak(payload)); terminate_session(*work); work_queue_guard.pop_front(); } @@ -309,7 +309,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->opcode == RSP_ACK) { if (payload->req_opcode == CMD_REMOVE_DIRECTORY) { stop_timer(); - call_callback(item.callback, ClientResult::Success); + item.callback(ClientResult::Success); work_queue_guard.pop_front(); } else { @@ -318,7 +318,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload)); + item.callback(result_from_nak(payload)); terminate_session(*work); work_queue_guard.pop_front(); } @@ -328,8 +328,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->req_opcode == CMD_CALC_FILE_CRC32) { stop_timer(); uint32_t remote_crc = *reinterpret_cast(payload->data); - call_callback( - item.callback, ClientResult::Success, remote_crc == item.local_crc); + item.callback(ClientResult::Success, remote_crc == item.local_crc); work_queue_guard.pop_front(); } else { @@ -338,7 +337,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) } else if (payload->opcode == RSP_NAK) { stop_timer(); - call_callback(item.callback, result_from_nak(payload), false); + item.callback(result_from_nak(payload), false); terminate_session(*work); work_queue_guard.pop_front(); } @@ -361,9 +360,9 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) stop_timer(); if (payload->data[0] == ERR_EOF) { std::sort(item.dirs.begin(), item.dirs.end()); - call_callback(item.callback, ClientResult::Success, item.dirs); + item.callback(ClientResult::Success, item.dirs); } else { - call_callback(item.callback, result_from_nak(payload), {}); + item.callback(result_from_nak(payload), {}); } terminate_session(*work); work_queue_guard.pop_front(); @@ -385,7 +384,7 @@ bool MavlinkFtpClient::download_start(Work& work, DownloadItem& item) item.ofstream.open(local_path, std::fstream::trunc | std::fstream::binary); if (!item.ofstream) { LogErr() << "Could not open it!"; - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); return false; } @@ -422,7 +421,7 @@ bool MavlinkFtpClient::download_continue(Work& work, DownloadItem& item, Payload if (item.bytes_transferred < item.file_size) { item.ofstream.write(reinterpret_cast(payload->data), payload->size); if (!item.ofstream) { - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); return false; } item.bytes_transferred += payload->size; @@ -432,8 +431,7 @@ bool MavlinkFtpClient::download_continue(Work& work, DownloadItem& item, Payload << " bytes"; } } - call_callback( - item.callback, + item.callback( ClientResult::Next, ProgressData{ static_cast(item.bytes_transferred), @@ -484,7 +482,7 @@ bool MavlinkFtpClient::download_burst_start(Work& work, DownloadBurstItem& item) item.ofstream.open(local_path, std::fstream::trunc | std::fstream::binary); if (!item.ofstream) { LogErr() << "Could not open it!"; - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); return false; } @@ -539,7 +537,7 @@ bool MavlinkFtpClient::download_burst_continue( item.ofstream.write(empty.data(), empty.size()); if (!item.ofstream) { LogWarn() << "Write failed"; - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); download_burst_end(work); return false; } @@ -549,7 +547,7 @@ bool MavlinkFtpClient::download_burst_continue( item.ofstream.write(reinterpret_cast(payload->data), payload->size); if (!item.ofstream) { LogWarn() << "Write failed"; - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); download_burst_end(work); return false; } @@ -578,8 +576,7 @@ bool MavlinkFtpClient::download_burst_continue( request_next_rest(work, item); } } else { - call_callback( - item.callback, + item.callback( ClientResult::Next, ProgressData{ static_cast(burst_bytes_transferred(item)), @@ -603,14 +600,14 @@ bool MavlinkFtpClient::download_burst_continue( item.ofstream.seekp(payload->offset); if (item.ofstream.fail()) { LogWarn() << "Seek failed"; - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); download_burst_end(work); return false; } item.ofstream.write(reinterpret_cast(payload->data), payload->size); if (!item.ofstream) { - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); download_burst_end(work); return false; } @@ -618,7 +615,7 @@ bool MavlinkFtpClient::download_burst_continue( auto& missing = item.missing_data.front(); if (missing.offset != payload->offset) { LogErr() << "Offset mismatch"; - call_callback(item.callback, ClientResult::ProtocolError, {}); + item.callback(ClientResult::ProtocolError, {}); download_burst_end(work); return false; } @@ -646,8 +643,7 @@ bool MavlinkFtpClient::download_burst_continue( // Final step download_burst_end(work); } else { - call_callback( - item.callback, + item.callback( ClientResult::Next, ProgressData{ static_cast(bytes_transferred), @@ -736,13 +732,13 @@ bool MavlinkFtpClient::upload_start(Work& work, UploadItem& item) { std::error_code ec; if (!fs::exists(item.local_file_path, ec)) { - call_callback(item.callback, ClientResult::FileDoesNotExist, {}); + item.callback(ClientResult::FileDoesNotExist, {}); return false; } item.ifstream.open(item.local_file_path, std::fstream::binary); if (!item.ifstream) { - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); return false; } @@ -757,7 +753,7 @@ bool MavlinkFtpClient::upload_start(Work& work, UploadItem& item) fs::path(item.remote_folder) / fs::path(item.local_file_path).filename(); if (remote_file_path.string().size() >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter, {}); + item.callback(ClientResult::InvalidParameter, {}); return false; } @@ -800,7 +796,7 @@ bool MavlinkFtpClient::upload_continue(Work& work, UploadItem& item) int bytes_read = item.ifstream.gcount(); if (!item.ifstream) { - call_callback(item.callback, ClientResult::FileIoError, {}); + item.callback(ClientResult::FileIoError, {}); return false; } @@ -826,8 +822,7 @@ bool MavlinkFtpClient::upload_continue(Work& work, UploadItem& item) send_mavlink_ftp_message(work.payload, work.target_compid); } - call_callback( - item.callback, + item.callback( ClientResult::Next, ProgressData{ static_cast(item.bytes_transferred), static_cast(item.file_size)}); @@ -838,7 +833,7 @@ bool MavlinkFtpClient::upload_continue(Work& work, UploadItem& item) bool MavlinkFtpClient::remove_start(Work& work, RemoveItem& item) { if (item.path.length() >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter); + item.callback(ClientResult::InvalidParameter); return false; } @@ -860,7 +855,7 @@ bool MavlinkFtpClient::remove_start(Work& work, RemoveItem& item) bool MavlinkFtpClient::rename_start(Work& work, RenameItem& item) { if (item.from_path.length() + item.to_path.length() + 1 >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter); + item.callback(ClientResult::InvalidParameter); return false; } @@ -888,7 +883,7 @@ bool MavlinkFtpClient::rename_start(Work& work, RenameItem& item) bool MavlinkFtpClient::create_dir_start(Work& work, CreateDirItem& item) { if (item.path.length() + 1 >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter); + item.callback(ClientResult::InvalidParameter); return false; } @@ -910,7 +905,7 @@ bool MavlinkFtpClient::create_dir_start(Work& work, CreateDirItem& item) bool MavlinkFtpClient::remove_dir_start(Work& work, RemoveDirItem& item) { if (item.path.length() + 1 >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter); + item.callback(ClientResult::InvalidParameter); return false; } @@ -932,13 +927,13 @@ bool MavlinkFtpClient::remove_dir_start(Work& work, RemoveDirItem& item) bool MavlinkFtpClient::compare_files_start(Work& work, CompareFilesItem& item) { if (item.remote_path.length() + 1 >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter, false); + item.callback(ClientResult::InvalidParameter, false); return false; } auto result_local = calc_local_file_crc32(item.local_path, item.local_crc); if (result_local != ClientResult::Success) { - call_callback(item.callback, result_local, false); + item.callback(result_local, false); return false; } @@ -961,7 +956,7 @@ bool MavlinkFtpClient::compare_files_start(Work& work, CompareFilesItem& item) bool MavlinkFtpClient::list_dir_start(Work& work, ListDirItem& item) { if (item.path.length() + 1 >= max_data_length) { - call_callback(item.callback, ClientResult::InvalidParameter, {}); + item.callback(ClientResult::InvalidParameter, {}); return false; } @@ -993,7 +988,7 @@ bool MavlinkFtpClient::list_dir_continue(Work& work, ListDirItem& item, PayloadH if (payload->size == 0) { std::sort(item.dirs.begin(), item.dirs.end()); - call_callback(item.callback, ClientResult::Success, item.dirs); + item.callback(ClientResult::Success, item.dirs); return false; } @@ -1220,7 +1215,7 @@ void MavlinkFtpClient::timeout() overloaded{ [&](DownloadItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout, {}); + item.callback(ClientResult::Timeout, {}); work_queue_guard.pop_front(); return; } @@ -1233,7 +1228,7 @@ void MavlinkFtpClient::timeout() }, [&](DownloadBurstItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout, {}); + item.callback(ClientResult::Timeout, {}); work_queue_guard.pop_front(); return; } @@ -1250,7 +1245,7 @@ void MavlinkFtpClient::timeout() if (item.current_offset == item.file_size && item.missing_data.empty()) { // We are done anyway. item.ofstream.close(); - call_callback(item.callback, ClientResult::Success, {}); + item.callback(ClientResult::Success, {}); download_burst_end(*work); work_queue_guard.pop_front(); } else { @@ -1277,7 +1272,7 @@ void MavlinkFtpClient::timeout() }, [&](UploadItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout, {}); + item.callback(ClientResult::Timeout, {}); work_queue_guard.pop_front(); return; } @@ -1290,7 +1285,7 @@ void MavlinkFtpClient::timeout() }, [&](RemoveItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout); + item.callback(ClientResult::Timeout); work_queue_guard.pop_front(); return; } @@ -1303,7 +1298,7 @@ void MavlinkFtpClient::timeout() }, [&](RenameItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout); + item.callback(ClientResult::Timeout); work_queue_guard.pop_front(); return; } @@ -1316,7 +1311,7 @@ void MavlinkFtpClient::timeout() }, [&](CreateDirItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout); + item.callback(ClientResult::Timeout); work_queue_guard.pop_front(); return; } @@ -1329,7 +1324,7 @@ void MavlinkFtpClient::timeout() }, [&](RemoveDirItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout); + item.callback(ClientResult::Timeout); work_queue_guard.pop_front(); return; } @@ -1342,7 +1337,7 @@ void MavlinkFtpClient::timeout() }, [&](CompareFilesItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout, false); + item.callback(ClientResult::Timeout, false); work_queue_guard.pop_front(); return; } @@ -1355,7 +1350,7 @@ void MavlinkFtpClient::timeout() }, [&](ListDirItem& item) { if (--work->retries == 0) { - call_callback(item.callback, ClientResult::Timeout, {}); + item.callback(ClientResult::Timeout, {}); work_queue_guard.pop_front(); return; } @@ -1464,22 +1459,4 @@ std::ostream& operator<<(std::ostream& str, MavlinkFtpClient::ClientResult const } } -template -void MavlinkFtpClient::call_callback( - const CallbackT& callback, - MavlinkFtpClient::ClientResult result, - const typename CallbackT::second_argument_type& extra_arg) -{ - _system_impl.call_user_callback( - [temp_callback = callback, temp_result = result, temp_extra_arg = extra_arg]() { - temp_callback(temp_result, temp_extra_arg); - }); -} -template -void MavlinkFtpClient::call_callback( - const CallbackT& callback, MavlinkFtpClient::ClientResult result) -{ - _system_impl.call_user_callback( - [temp_callback = callback, temp_result = result]() { temp_callback(temp_result); }); -} } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_ftp_client.h b/src/mavsdk/core/mavlink_ftp_client.h index 1cce540de..cb62635f7 100644 --- a/src/mavsdk/core/mavlink_ftp_client.h +++ b/src/mavsdk/core/mavlink_ftp_client.h @@ -299,13 +299,6 @@ class MavlinkFtpClient { void terminate_session(Work& work); - template void call_callback(const CallbackT& callback, ClientResult result); - template - void call_callback( - const CallbackT& callback, - ClientResult result, - const typename CallbackT::second_argument_type& extra_arg); - static ClientResult result_from_nak(PayloadHeader* payload); void timeout();