From 96f6cac1cb6ccc4cd3dd8aead1d413527162914c Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Thu, 23 Dec 2021 14:35:17 +0100 Subject: [PATCH 1/7] examples: add camera_manager --- examples/camera_manager/CMakeLists.txt | 25 + examples/camera_manager/camera_manager.cpp | 122 ++++ examples/camera_manager/infos/camera_info.xml | 664 ++++++++++++++++++ 3 files changed, 811 insertions(+) create mode 100644 examples/camera_manager/CMakeLists.txt create mode 100644 examples/camera_manager/camera_manager.cpp create mode 100644 examples/camera_manager/infos/camera_info.xml diff --git a/examples/camera_manager/CMakeLists.txt b/examples/camera_manager/CMakeLists.txt new file mode 100644 index 0000000000..dac2bcdb6e --- /dev/null +++ b/examples/camera_manager/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.10.2) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +project(camera_manager) + +add_executable(camera_manager + camera_manager.cpp +) + +find_package(MAVSDK REQUIRED) + +target_link_libraries(camera_manager + MAVSDK::mavsdk_ftp + MAVSDK::mavsdk_mavlink_passthrough + MAVSDK::mavsdk_param_server + MAVSDK::mavsdk +) + +if(NOT MSVC) + add_compile_options(camera_manager PRIVATE -Wall -Wextra) +else() + add_compile_options(camera_manager PRIVATE -WX -W2) +endif() diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp new file mode 100644 index 0000000000..71943346b2 --- /dev/null +++ b/examples/camera_manager/camera_manager.cpp @@ -0,0 +1,122 @@ +// +// Example of a camera manager with MAVSDK. +// + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace mavsdk; +using std::chrono::seconds; +using std::this_thread::sleep_for; + +void usage(const std::string& bin_name) +{ + std::cerr << "Usage : " << bin_name << " \n" + << '\n' + << " Start mavlink camera_manager with camera definition file in \n" +} + +int main(int argc, char** argv) +{ + if (argc != 2) { + usage(argv[0]); + return 1; + } + + Mavsdk mavsdk; + Mavsdk::Configuration configuration(Mavsdk::Configuration::UsageType::Camera); + mavsdk.set_configuration(configuration); + ConnectionResult connection_result = mavsdk.add_any_connection("udp://:24547"); + if (connection_result != ConnectionResult::Success) { + std::cerr << "Error setting up Mavlink FTP server.\n"; + return 1; + } + + auto prom = std::promise>{}; + auto fut = prom.get_future(); + // We wait for new systems to be discovered, once we find one that has an + // autopilot, we decide to use it. + mavsdk.subscribe_on_new_system([&mavsdk, &prom]() { + auto system = mavsdk.systems().back(); + + // Unsubscribe again as we only want to find one system. + mavsdk.subscribe_on_new_system(nullptr); + prom.set_value(system); + }); + + // We usually receive heartbeats at 1Hz, therefore we should find a + // system after around 3 minutes max, surely. + if (fut.wait_for(seconds(180)) == std::future_status::timeout) { + std::cerr << "No GCS found.\n"; + return 1; + } + + std::cout << "No GCS found.\n"; + + // Get discovered system now. + auto system = fut.get(); + + auto ftp_server = Ftp{system}; + ftp_server.set_root_directory(argv[1]); + + auto parameter_server = ParamServer{system}; + parameter_server.provide_param_int("CAM_ISO", 1); + parameter_server.provide_param_float("CAM_EV", -0.3f); + + auto mavlink_passthrough = MavlinkPassthrough{system}; + mavlink_passthrough.subscribe_message_async( + MAVLINK_MSG_ID_COMMAND_LONG, + [&](const mavlink_message_t& msg) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&msg, &cmd); + switch (cmd.command) { + case MAV_CMD_REQUEST_CAMERA_INFORMATION: + mavlink_camera_information_t camera_information; + camera_information.time_boot_ms = 0; + strncpy((char*)camera_information.vendor_name, "Foo Industries", 32); + strncpy((char*)camera_information.model_name, "T100", 32); + camera_information.firmware_version = 0; + camera_information.focal_length = 16; + camera_information.sensor_size_h = 23.2; + camera_information.sensor_size_v = 15.4; + camera_information.resolution_h = 5456; + camera_information.resolution_v = 3632; + camera_information.flags = + CAMERA_CAP_FLAGS_CAPTURE_VIDEO | + CAMERA_CAP_FLAGS_CAPTURE_IMAGE | + CAMERA_CAP_FLAGS_HAS_MODES | + CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE | + CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE | + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM | + CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS | + CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; + camera_information.cam_definition_version = 0; + strncpy(camera_information.cam_definition_uri, "mavlinkftp:///infos/camera_info.xml", 140); + std::cout << "Sending CAMERA_INFORMATION" << std::endl; + mavlink_message_t send_msg; + mavlink_msg_camera_information_encode(mavlink_passthrough.get_our_sysid(), mavlink_passthrough.get_our_compid(), &send_msg, &camera_information); + mavlink_passthrough.send_message(send_msg); + break; + }; + } + ); + + std::cout << "Mavlink FTP server running.\n" + << '\n' + << "Component ID: " << static_cast(ftp_server.get_our_compid()) << '\n'; + + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + return 0; +} diff --git a/examples/camera_manager/infos/camera_info.xml b/examples/camera_manager/infos/camera_info.xml new file mode 100644 index 0000000000..0ea431c3b1 --- /dev/null +++ b/examples/camera_manager/infos/camera_info.xml @@ -0,0 +1,664 @@ + + + + T100 + Foo Industries + + + + + Camera Mode + + + CAM_SHUTTERSPD + CAM_ISO + CAM_VIDRES + + + + + + + + White Balance Mode + + + + + Exposure Mode + + CAM_SHUTTERSPD + CAM_ISO + + + + + + + + Shutter Speed + + + + + ISO + + + + + Exposure Compensation + + + + + Video Resolution + + CAM_SHUTTERSPD + CAM_ISO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Video Format + + CAM_SHUTTERSPD + CAM_ISO + CAM_VIDRES + + + + + + + Color Mode + + + + + Image Format + + + + + Image Quality + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From cc4522c02a685074641a0498da759e5dece26e2e Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Sat, 25 Dec 2021 10:28:20 +0100 Subject: [PATCH 2/7] ftp plugin: handle burst operation --- examples/camera_manager/camera_manager.cpp | 6 +- src/mavsdk/plugins/ftp/ftp_impl.cpp | 105 +++++++++++++++++++-- src/mavsdk/plugins/ftp/ftp_impl.h | 4 +- 3 files changed, 101 insertions(+), 14 deletions(-) diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp index 71943346b2..225ad1d323 100644 --- a/examples/camera_manager/camera_manager.cpp +++ b/examples/camera_manager/camera_manager.cpp @@ -22,7 +22,7 @@ void usage(const std::string& bin_name) { std::cerr << "Usage : " << bin_name << " \n" << '\n' - << " Start mavlink camera_manager with camera definition file in \n" + << " Start mavlink camera_manager with camera definition file in " << std::endl; } int main(int argc, char** argv) @@ -60,8 +60,6 @@ int main(int argc, char** argv) return 1; } - std::cout << "No GCS found.\n"; - // Get discovered system now. auto system = fut.get(); @@ -100,7 +98,7 @@ int main(int argc, char** argv) CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS | CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; camera_information.cam_definition_version = 0; - strncpy(camera_information.cam_definition_uri, "mavlinkftp:///infos/camera_info.xml", 140); + strncpy(camera_information.cam_definition_uri, "mftp://infos/camera_info.xml", 140); std::cout << "Sending CAMERA_INFORMATION" << std::endl; mavlink_message_t send_msg; mavlink_msg_camera_information_encode(mavlink_passthrough.get_our_sysid(), mavlink_passthrough.get_our_compid(), &send_msg, &camera_information); diff --git a/src/mavsdk/plugins/ftp/ftp_impl.cpp b/src/mavsdk/plugins/ftp/ftp_impl.cpp index 8fcdcf95a6..13c4906350 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.cpp +++ b/src/mavsdk/plugins/ftp/ftp_impl.cpp @@ -36,6 +36,7 @@ FtpImpl::~FtpImpl() void FtpImpl::init() { + LogDebug() << "register mavlink message handler"; _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, [this](const mavlink_message_t& message) { process_mavlink_ftp_message(message); }, @@ -762,7 +763,7 @@ void FtpImpl::process_mavlink_ftp_message(const mavlink_message_t& msg) mavlink_msg_file_transfer_protocol_decode(&msg, &ftp_req); if (ftp_req.target_component != 0 && ftp_req.target_component != get_our_compid()) { - LogWarn() << "wrong compid!"; + LogWarn() << "wrong compid! Requested: " << (int)ftp_req.target_component << " Our ID: " << (int)get_our_compid(); return; } @@ -774,11 +775,9 @@ void FtpImpl::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->size > max_data_length) { error_code = ServerResult::ERR_INVALID_DATA_SIZE; } else { - /* - LogDebug() << "ftp - opc: " << (int)payload->opcode << " size: " - << (int)payload->size << " offset: " << (int)payload->offset << " seq: " << - payload->seq_number; - */ + LogDebug() << "ftp - opc: " << (int)payload->opcode << " size: " + << (int)payload->size << " offset: " << (int)payload->offset << " seq: " + << payload->seq_number; // check the sequence number: if this is a resent request, resend the last response if (_last_reply_valid) { @@ -832,7 +831,7 @@ void FtpImpl::process_mavlink_ftp_message(const mavlink_message_t& msg) case CMD_BURST_READ_FILE: LogInfo() << "OPC:CMD_BURST_READ_FILE"; - error_code = _work_burst(payload); + error_code = _work_burst(payload, msg.sysid, msg.compid); stream_send = true; break; @@ -1099,7 +1098,7 @@ FtpImpl::ServerResult FtpImpl::_work_read(PayloadHeader* payload) return ServerResult::SUCCESS; } -FtpImpl::ServerResult FtpImpl::_work_burst(PayloadHeader* payload) +FtpImpl::ServerResult FtpImpl::_work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id) { if (payload->session != 0 && _session_info.fd < 0) { return ServerResult::ERR_INVALID_SESSION; @@ -1110,7 +1109,10 @@ FtpImpl::ServerResult FtpImpl::_work_burst(PayloadHeader* payload) _session_info.stream_offset = payload->offset; _session_info.stream_chunk_transmitted = 0; _session_info.stream_seq_number = payload->seq_number + 1; - _session_info.stream_target_system_id = _parent->get_system_id(); + _session_info.stream_target_system_id = target_system_id; + _session_info.stream_target_component_id = target_component_id; + + send(); return ServerResult::SUCCESS; } @@ -1306,12 +1308,97 @@ FtpImpl::ServerResult FtpImpl::_work_calc_file_CRC32(PayloadHeader* payload) return ServerResult::SUCCESS; } +unsigned +FtpImpl::get_size() +{ + if (_session_info.stream_download) { + return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void FtpImpl::send() { // Anything to stream? if (!_session_info.stream_download) { return; } + + unsigned max_bytes_to_send = 20000; // _mavlink->get_free_tx_buf(); + + // Send stream packets until buffer is full + bool more_data = false; + + ServerResult error_code = ServerResult::SUCCESS; + mavlink_file_transfer_protocol_t ftp_msg; + PayloadHeader *payload = reinterpret_cast(&ftp_msg.payload[0]); + payload->seq_number = _session_info.stream_seq_number; + payload->session = 0; + payload->opcode = RSP_ACK; + payload->req_opcode = CMD_BURST_READ_FILE; + payload->offset = _session_info.stream_offset; + _session_info.stream_seq_number++; + // We have to test seek past EOF ourselves, lseek will allow seek past EOF + if (_session_info.stream_offset >= _session_info.file_size) { + error_code = ServerResult::ERR_EOF; + } + + if (error_code == ServerResult::SUCCESS) { + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + error_code = ServerResult::ERR_FAIL; + } + } + + if (error_code == ServerResult::SUCCESS) { + int bytes_read = ::read(_session_info.fd, &payload->data[0], max_data_length); + + if (bytes_read < 0) { + // Negative return indicates error other than eof + error_code = ServerResult::ERR_FAIL; + } else { + payload->size = bytes_read; + _session_info.stream_offset += bytes_read; + _session_info.stream_chunk_transmitted += bytes_read; + LogDebug() << "Burst step: " << _session_info.stream_offset << " < " << _session_info.file_size; + more_data = (_session_info.stream_offset < _session_info.file_size); + if (!more_data) { + LogDebug() << "Burst completed"; + payload->burst_complete = true; + _session_info.stream_download = false; + _session_info.stream_chunk_transmitted = 0; + + } else { + more_data = true; + payload->burst_complete = false; + max_bytes_to_send -= get_size(); + } + } + } + + if (error_code != ServerResult::SUCCESS) { + payload->opcode = RSP_NAK; + payload->size = 1; + uint8_t *pData = &payload->data[0]; + *pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error + + if (error_code == ServerResult::ERR_FAIL_ERRNO) { + payload->size = 2; + payload->data[1] = errno; + } + + _session_info.stream_download = false; + + } + ftp_msg.target_system = _session_info.stream_target_system_id; + ftp_msg.target_network = 0; + ftp_msg.target_component = _get_target_component_id(); + mavlink_message_t msg; + mavlink_msg_file_transfer_protocol_encode(_parent->get_own_system_id(), _parent->get_own_component_id(), &msg, &ftp_msg); + _parent->send_message(msg); + + if (more_data) send(); } } // namespace mavsdk diff --git a/src/mavsdk/plugins/ftp/ftp_impl.h b/src/mavsdk/plugins/ftp/ftp_impl.h index 05d31be84e..f138360406 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.h +++ b/src/mavsdk/plugins/ftp/ftp_impl.h @@ -150,6 +150,7 @@ class FtpImpl : public PluginImplBase { uint32_t stream_offset{0}; uint16_t stream_seq_number{0}; uint8_t stream_target_system_id{0}; + uint8_t stream_target_component_id{0}; unsigned stream_chunk_transmitted{0}; }; @@ -216,6 +217,7 @@ class FtpImpl : public PluginImplBase { void _reset_timer(); void _stop_timer(); void _list_directory(uint32_t offset); + unsigned get_size(); uint8_t _get_target_component_id() { return _target_component_id_set ? _target_component_id : _parent->get_autopilot_id(); @@ -238,7 +240,7 @@ class FtpImpl : public PluginImplBase { ServerResult _work_list(PayloadHeader* payload, bool list_hidden = false); ServerResult _work_open(PayloadHeader* payload, int oflag); ServerResult _work_read(PayloadHeader* payload); - ServerResult _work_burst(PayloadHeader* payload); + ServerResult _work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id); ServerResult _work_write(PayloadHeader* payload); ServerResult _work_terminate(PayloadHeader* payload); ServerResult _work_reset(PayloadHeader* payload); From 84a9b1321f6f0dad36f894e8cbea3af363c001b9 Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Sat, 25 Dec 2021 10:31:04 +0100 Subject: [PATCH 3/7] camera_manager: use correct mftp url --- examples/camera_manager/camera_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp index 225ad1d323..c120875fc2 100644 --- a/examples/camera_manager/camera_manager.cpp +++ b/examples/camera_manager/camera_manager.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv) CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS | CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; camera_information.cam_definition_version = 0; - strncpy(camera_information.cam_definition_uri, "mftp://infos/camera_info.xml", 140); + strncpy(camera_information.cam_definition_uri, "mftp://[;compid=100]infos/camera_info.xml", 140); std::cout << "Sending CAMERA_INFORMATION" << std::endl; mavlink_message_t send_msg; mavlink_msg_camera_information_encode(mavlink_passthrough.get_our_sysid(), mavlink_passthrough.get_our_compid(), &send_msg, &camera_information); From 2bfc92eeae6d433f886fd7ac8b39cad17d194c05 Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Mon, 27 Dec 2021 10:04:48 +0100 Subject: [PATCH 4/7] fix style --- examples/camera_manager/camera_manager.cpp | 66 ++++++++++++---------- src/mavsdk/plugins/ftp/ftp_impl.cpp | 47 +++++++-------- src/mavsdk/plugins/ftp/ftp_impl.h | 3 +- 3 files changed, 62 insertions(+), 54 deletions(-) diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp index c120875fc2..7127b7bd8c 100644 --- a/examples/camera_manager/camera_manager.cpp +++ b/examples/camera_manager/camera_manager.cpp @@ -22,7 +22,8 @@ void usage(const std::string& bin_name) { std::cerr << "Usage : " << bin_name << " \n" << '\n' - << " Start mavlink camera_manager with camera definition file in " << std::endl; + << " Start mavlink camera_manager with camera definition file in " + << std::endl; } int main(int argc, char** argv) @@ -72,41 +73,44 @@ int main(int argc, char** argv) auto mavlink_passthrough = MavlinkPassthrough{system}; mavlink_passthrough.subscribe_message_async( - MAVLINK_MSG_ID_COMMAND_LONG, - [&](const mavlink_message_t& msg) { - mavlink_command_long_t cmd; + MAVLINK_MSG_ID_COMMAND_LONG, [&](const mavlink_message_t& msg) { + mavlink_command_long_t cmd; mavlink_msg_command_long_decode(&msg, &cmd); - switch (cmd.command) { - case MAV_CMD_REQUEST_CAMERA_INFORMATION: - mavlink_camera_information_t camera_information; - camera_information.time_boot_ms = 0; - strncpy((char*)camera_information.vendor_name, "Foo Industries", 32); - strncpy((char*)camera_information.model_name, "T100", 32); - camera_information.firmware_version = 0; - camera_information.focal_length = 16; - camera_information.sensor_size_h = 23.2; - camera_information.sensor_size_v = 15.4; - camera_information.resolution_h = 5456; - camera_information.resolution_v = 3632; - camera_information.flags = - CAMERA_CAP_FLAGS_CAPTURE_VIDEO | - CAMERA_CAP_FLAGS_CAPTURE_IMAGE | + switch (cmd.command) { + case MAV_CMD_REQUEST_CAMERA_INFORMATION: + mavlink_camera_information_t camera_information; + camera_information.time_boot_ms = 0; + strncpy((char*)camera_information.vendor_name, "Foo Industries", 32); + strncpy((char*)camera_information.model_name, "T100", 32); + camera_information.firmware_version = 0; + camera_information.focal_length = 16; + camera_information.sensor_size_h = 23.2; + camera_information.sensor_size_v = 15.4; + camera_information.resolution_h = 5456; + camera_information.resolution_v = 3632; + camera_information.flags = + CAMERA_CAP_FLAGS_CAPTURE_VIDEO | CAMERA_CAP_FLAGS_CAPTURE_IMAGE | CAMERA_CAP_FLAGS_HAS_MODES | CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE | CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE | - CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM | - CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS | + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM | CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS | CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; - camera_information.cam_definition_version = 0; - strncpy(camera_information.cam_definition_uri, "mftp://[;compid=100]infos/camera_info.xml", 140); - std::cout << "Sending CAMERA_INFORMATION" << std::endl; - mavlink_message_t send_msg; - mavlink_msg_camera_information_encode(mavlink_passthrough.get_our_sysid(), mavlink_passthrough.get_our_compid(), &send_msg, &camera_information); - mavlink_passthrough.send_message(send_msg); - break; - }; - } - ); + camera_information.cam_definition_version = 0; + strncpy( + camera_information.cam_definition_uri, + "mftp://[;compid=100]infos/camera_info.xml", + 140); + std::cout << "Sending CAMERA_INFORMATION" << std::endl; + mavlink_message_t send_msg; + mavlink_msg_camera_information_encode( + mavlink_passthrough.get_our_sysid(), + mavlink_passthrough.get_our_compid(), + &send_msg, + &camera_information); + mavlink_passthrough.send_message(send_msg); + break; + }; + }); std::cout << "Mavlink FTP server running.\n" << '\n' diff --git a/src/mavsdk/plugins/ftp/ftp_impl.cpp b/src/mavsdk/plugins/ftp/ftp_impl.cpp index 13c4906350..301493a11a 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.cpp +++ b/src/mavsdk/plugins/ftp/ftp_impl.cpp @@ -763,7 +763,8 @@ void FtpImpl::process_mavlink_ftp_message(const mavlink_message_t& msg) mavlink_msg_file_transfer_protocol_decode(&msg, &ftp_req); if (ftp_req.target_component != 0 && ftp_req.target_component != get_our_compid()) { - LogWarn() << "wrong compid! Requested: " << (int)ftp_req.target_component << " Our ID: " << (int)get_our_compid(); + LogWarn() << "wrong compid! Requested: " << (int)ftp_req.target_component + << " Our ID: " << (int)get_our_compid(); return; } @@ -775,9 +776,8 @@ void FtpImpl::process_mavlink_ftp_message(const mavlink_message_t& msg) if (payload->size > max_data_length) { error_code = ServerResult::ERR_INVALID_DATA_SIZE; } else { - LogDebug() << "ftp - opc: " << (int)payload->opcode << " size: " - << (int)payload->size << " offset: " << (int)payload->offset << " seq: " - << payload->seq_number; + LogDebug() << "ftp - opc: " << (int)payload->opcode << " size: " << (int)payload->size + << " offset: " << (int)payload->offset << " seq: " << payload->seq_number; // check the sequence number: if this is a resent request, resend the last response if (_last_reply_valid) { @@ -1098,7 +1098,8 @@ FtpImpl::ServerResult FtpImpl::_work_read(PayloadHeader* payload) return ServerResult::SUCCESS; } -FtpImpl::ServerResult FtpImpl::_work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id) +FtpImpl::ServerResult +FtpImpl::_work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id) { if (payload->session != 0 && _session_info.fd < 0) { return ServerResult::ERR_INVALID_SESSION; @@ -1308,15 +1309,14 @@ FtpImpl::ServerResult FtpImpl::_work_calc_file_CRC32(PayloadHeader* payload) return ServerResult::SUCCESS; } -unsigned -FtpImpl::get_size() +unsigned FtpImpl::get_size() { - if (_session_info.stream_download) { - return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (_session_info.stream_download) { + return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } else { - return 0; - } + } else { + return 0; + } } void FtpImpl::send() @@ -1333,7 +1333,7 @@ void FtpImpl::send() ServerResult error_code = ServerResult::SUCCESS; mavlink_file_transfer_protocol_t ftp_msg; - PayloadHeader *payload = reinterpret_cast(&ftp_msg.payload[0]); + PayloadHeader* payload = reinterpret_cast(&ftp_msg.payload[0]); payload->seq_number = _session_info.stream_seq_number; payload->session = 0; payload->opcode = RSP_ACK; @@ -1361,7 +1361,8 @@ void FtpImpl::send() payload->size = bytes_read; _session_info.stream_offset += bytes_read; _session_info.stream_chunk_transmitted += bytes_read; - LogDebug() << "Burst step: " << _session_info.stream_offset << " < " << _session_info.file_size; + LogDebug() << "Burst step: " << _session_info.stream_offset << " < " + << _session_info.file_size; more_data = (_session_info.stream_offset < _session_info.file_size); if (!more_data) { LogDebug() << "Burst completed"; @@ -1372,7 +1373,7 @@ void FtpImpl::send() } else { more_data = true; payload->burst_complete = false; - max_bytes_to_send -= get_size(); + max_bytes_to_send -= get_size(); } } } @@ -1380,25 +1381,27 @@ void FtpImpl::send() if (error_code != ServerResult::SUCCESS) { payload->opcode = RSP_NAK; payload->size = 1; - uint8_t *pData = &payload->data[0]; - *pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error + uint8_t* pData = &payload->data[0]; + *pData = + error_code; // Straight reference to data[0] is causing bogus gcc array subscript error if (error_code == ServerResult::ERR_FAIL_ERRNO) { - payload->size = 2; - payload->data[1] = errno; + payload->size = 2; + payload->data[1] = errno; } _session_info.stream_download = false; - } ftp_msg.target_system = _session_info.stream_target_system_id; ftp_msg.target_network = 0; ftp_msg.target_component = _get_target_component_id(); mavlink_message_t msg; - mavlink_msg_file_transfer_protocol_encode(_parent->get_own_system_id(), _parent->get_own_component_id(), &msg, &ftp_msg); + mavlink_msg_file_transfer_protocol_encode( + _parent->get_own_system_id(), _parent->get_own_component_id(), &msg, &ftp_msg); _parent->send_message(msg); - if (more_data) send(); + if (more_data) + send(); } } // namespace mavsdk diff --git a/src/mavsdk/plugins/ftp/ftp_impl.h b/src/mavsdk/plugins/ftp/ftp_impl.h index f138360406..15388c0cf7 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.h +++ b/src/mavsdk/plugins/ftp/ftp_impl.h @@ -240,7 +240,8 @@ class FtpImpl : public PluginImplBase { ServerResult _work_list(PayloadHeader* payload, bool list_hidden = false); ServerResult _work_open(PayloadHeader* payload, int oflag); ServerResult _work_read(PayloadHeader* payload); - ServerResult _work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id); + ServerResult + _work_burst(PayloadHeader* payload, uint8_t target_system_id, uint8_t target_component_id); ServerResult _work_write(PayloadHeader* payload); ServerResult _work_terminate(PayloadHeader* payload); ServerResult _work_reset(PayloadHeader* payload); From bbe142c512ff29c5401d0510f9803793acca839c Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Fri, 14 Jan 2022 14:16:35 +0100 Subject: [PATCH 5/7] applied reviewer suggestions --- examples/camera_manager/CMakeLists.txt | 3 - examples/camera_manager/camera_manager.cpp | 6 +- .../plugins/camera_manager/CMakeLists.txt | 25 ++++++ .../camera_manager/camera_manager_impl.cpp | 70 ++++++++++++++++ .../plugins/camera_manager/camera_manager.h | 83 +++++++++++++++++++ src/mavsdk/plugins/ftp/ftp_impl.cpp | 4 +- src/mavsdk/plugins/ftp/ftp_impl.h | 2 +- 7 files changed, 184 insertions(+), 9 deletions(-) create mode 100644 src/mavsdk/plugins/camera_manager/CMakeLists.txt create mode 100644 src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp create mode 100644 src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h diff --git a/examples/camera_manager/CMakeLists.txt b/examples/camera_manager/CMakeLists.txt index dac2bcdb6e..0f83728af7 100644 --- a/examples/camera_manager/CMakeLists.txt +++ b/examples/camera_manager/CMakeLists.txt @@ -12,9 +12,6 @@ add_executable(camera_manager find_package(MAVSDK REQUIRED) target_link_libraries(camera_manager - MAVSDK::mavsdk_ftp - MAVSDK::mavsdk_mavlink_passthrough - MAVSDK::mavsdk_param_server MAVSDK::mavsdk ) diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp index 7127b7bd8c..7b10c20b6b 100644 --- a/examples/camera_manager/camera_manager.cpp +++ b/examples/camera_manager/camera_manager.cpp @@ -38,7 +38,7 @@ int main(int argc, char** argv) mavsdk.set_configuration(configuration); ConnectionResult connection_result = mavsdk.add_any_connection("udp://:24547"); if (connection_result != ConnectionResult::Success) { - std::cerr << "Error setting up Mavlink FTP server.\n"; + std::cerr << "Error setting up Mavlink camera manager.\n"; return 1; } @@ -80,8 +80,8 @@ int main(int argc, char** argv) case MAV_CMD_REQUEST_CAMERA_INFORMATION: mavlink_camera_information_t camera_information; camera_information.time_boot_ms = 0; - strncpy((char*)camera_information.vendor_name, "Foo Industries", 32); - strncpy((char*)camera_information.model_name, "T100", 32); + strncpy((char*)camera_information.vendor_name, "Foo Industries", sizeof(camera_information.vendor_name)); + strncpy((char*)camera_information.model_name, "T100", sizeof(camera_information.model_name)); camera_information.firmware_version = 0; camera_information.focal_length = 16; camera_information.sensor_size_h = 23.2; diff --git a/src/mavsdk/plugins/camera_manager/CMakeLists.txt b/src/mavsdk/plugins/camera_manager/CMakeLists.txt new file mode 100644 index 0000000000..679209fecc --- /dev/null +++ b/src/mavsdk/plugins/camera_manager/CMakeLists.txt @@ -0,0 +1,25 @@ +target_sources(mavsdk + PRIVATE + # camera_manager.cpp + camera_manager_impl.cpp + # camera_definition_files/generated/camera_definition_files.cpp +) + +target_include_directories(mavsdk + # PRIVATE + # $ + PUBLIC + $ + $ + ) + +install(FILES + include/plugins/camera_manager/camera_manager.h + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/camera_manager +) + +#list(APPEND UNIT_TEST_SOURCES +# ${CMAKE_CURRENT_SOURCE_DIR}/camera_manager_test.cpp +#) +#set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE) + diff --git a/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp b/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp new file mode 100644 index 0000000000..14b65cd4d6 --- /dev/null +++ b/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp @@ -0,0 +1,70 @@ +/*#include "plugins/camera_manager/camera_manager.h" + +class CameraManagerPluginImpl : public PluginImplBase { +public: + explicit CameraManagerPluginImpl(System& system) : PluginImplBase(system) + { + _parent->register_plugin(this); + } + explicit CameraManagerPluginImpl(std::shared_ptr system) : PluginImplBase(system) + { + _parent->register_plugin(this); + } + ~CameraManagerPluginImpl() { _parent->unregister_plugin(this); } + + void init() override; + void deinit() override; + + void enable() override; + void disable() override; + + void provide_camera_information(const std::function<(mavlink_camera_information_t& camera_information)>& callback) { + callback(camera_information); + camera_information_valid = true; + } + + template + void add_param(const std::string& name, T default_value, const typename std::function& callback) { + _parent->provide_server_param_int("CAM_ISO", default_value); + _parent->subscribe_param( + name, + overloaded { + [&name](auto value) { std::cerr << "invalid parameter type for " << name << std::endl; }, + callback + }, + nullptr); + } + +private: + mavlink_camera_information_t camera_information; + bool camera_information_valid{false}; +}; + +void CameraManagerPluginImpl::init() { + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_LONG, + [this](const mavlink_message_t& message) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&msg, &cmd); + switch (cmd.command) { + case MAV_CMD_REQUEST_CAMERA_INFORMATION: + if (camera_information_valid) { + mavlink_message_t msg; + mavlink_msg_camera_information_encode( + _parent->get_own_system_id(), + _parent->get_own_component_id(), + &msg, + &camera_information); + _parent->send_message(msg); + } + break; + default: + break; + } + }, + this); +} +void CameraManagerPluginImpl::deinit() {} +void CameraManagerPluginImpl::enable() {} +void CameraManagerPluginImpl::disable() {} +*/ diff --git a/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h b/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h new file mode 100644 index 0000000000..cad4f09edd --- /dev/null +++ b/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "plugin_base.h" +#include "mavlink_include.h" + +namespace mavsdk { + +class CameraManagerPluginImpl : public PluginImplBase { +public: + explicit CameraManagerPluginImpl(System& system) : PluginImplBase(system) + { + _parent->register_plugin(this); + } + explicit CameraManagerPluginImpl(std::shared_ptr system) : PluginImplBase(system) + { + _parent->register_plugin(this); + } + ~CameraManagerPluginImpl() { _parent->unregister_plugin(this); } + + void init() override; + void deinit() override; + + void enable() override; + void disable() override; + + void provide_camera_information(const std::function<(mavlink_camera_information_t& camera_information)>& callback) { + callback(camera_information); + camera_information_valid = true; + } + + template + void add_param(const std::string& name, T default_value, const typename std::function& callback) { + _parent->provide_server_param_int("CAM_ISO", default_value); + _parent->subscribe_param( + name, + overloaded { + [&name](auto value) { std::cerr << "invalid parameter type for " << name << std::endl; }, + callback + }, + nullptr); + } + +private: + mavlink_camera_information_t camera_information; + bool camera_information_valid{false}; +}; + +void CameraManagerPluginImpl::init() { + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_LONG, + [this](const mavlink_message_t& message) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&msg, &cmd); + switch (cmd.command) { + case MAV_CMD_REQUEST_CAMERA_INFORMATION: + if (camera_information_valid) { + mavlink_message_t msg; + mavlink_msg_camera_information_encode( + _parent->get_own_system_id(), + _parent->get_own_component_id(), + &msg, + &camera_information); + _parent->send_message(msg); + } + break; + default: + break; + } + }, + this); +} +void CameraManagerPluginImpl::deinit() {} +void CameraManagerPluginImpl::enable() {} +void CameraManagerPluginImpl::disable() {} + +} diff --git a/src/mavsdk/plugins/ftp/ftp_impl.cpp b/src/mavsdk/plugins/ftp/ftp_impl.cpp index 301493a11a..f2062003aa 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.cpp +++ b/src/mavsdk/plugins/ftp/ftp_impl.cpp @@ -36,7 +36,6 @@ FtpImpl::~FtpImpl() void FtpImpl::init() { - LogDebug() << "register mavlink message handler"; _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, [this](const mavlink_message_t& message) { process_mavlink_ftp_message(message); }, @@ -1400,8 +1399,9 @@ void FtpImpl::send() _parent->get_own_system_id(), _parent->get_own_component_id(), &msg, &ftp_msg); _parent->send_message(msg); - if (more_data) + if (more_data) { send(); + } } } // namespace mavsdk diff --git a/src/mavsdk/plugins/ftp/ftp_impl.h b/src/mavsdk/plugins/ftp/ftp_impl.h index 15388c0cf7..a3951b5801 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.h +++ b/src/mavsdk/plugins/ftp/ftp_impl.h @@ -217,7 +217,7 @@ class FtpImpl : public PluginImplBase { void _reset_timer(); void _stop_timer(); void _list_directory(uint32_t offset); - unsigned get_size(); + unsigned get_size() const; uint8_t _get_target_component_id() { return _target_component_id_set ? _target_component_id : _parent->get_autopilot_id(); From 895bb4c8fb6e57529e5fa1845d92c2d0ce6b2cdb Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Fri, 14 Jan 2022 14:18:48 +0100 Subject: [PATCH 6/7] Make ftp_impl get_size a const function --- src/mavsdk/plugins/ftp/ftp_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/ftp/ftp_impl.cpp b/src/mavsdk/plugins/ftp/ftp_impl.cpp index f2062003aa..15aa2b98fc 100644 --- a/src/mavsdk/plugins/ftp/ftp_impl.cpp +++ b/src/mavsdk/plugins/ftp/ftp_impl.cpp @@ -1308,7 +1308,7 @@ FtpImpl::ServerResult FtpImpl::_work_calc_file_CRC32(PayloadHeader* payload) return ServerResult::SUCCESS; } -unsigned FtpImpl::get_size() +unsigned FtpImpl::get_size() const { if (_session_info.stream_download) { return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; From adc2ed281583db236055e12b07f7c2e5df2a16e5 Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Fri, 14 Jan 2022 14:40:07 +0100 Subject: [PATCH 7/7] fixed style --- examples/camera_manager/camera_manager.cpp | 10 ++++++-- .../camera_manager/camera_manager_impl.cpp | 13 +++++----- .../plugins/camera_manager/camera_manager.h | 24 ++++++++++++------- 3 files changed, 29 insertions(+), 18 deletions(-) diff --git a/examples/camera_manager/camera_manager.cpp b/examples/camera_manager/camera_manager.cpp index 7b10c20b6b..e1c0902756 100644 --- a/examples/camera_manager/camera_manager.cpp +++ b/examples/camera_manager/camera_manager.cpp @@ -80,8 +80,14 @@ int main(int argc, char** argv) case MAV_CMD_REQUEST_CAMERA_INFORMATION: mavlink_camera_information_t camera_information; camera_information.time_boot_ms = 0; - strncpy((char*)camera_information.vendor_name, "Foo Industries", sizeof(camera_information.vendor_name)); - strncpy((char*)camera_information.model_name, "T100", sizeof(camera_information.model_name)); + strncpy( + (char*)camera_information.vendor_name, + "Foo Industries", + sizeof(camera_information.vendor_name)); + strncpy( + (char*)camera_information.model_name, + "T100", + sizeof(camera_information.model_name)); camera_information.firmware_version = 0; camera_information.focal_length = 16; camera_information.sensor_size_h = 23.2; diff --git a/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp b/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp index 14b65cd4d6..80ad14bb84 100644 --- a/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp +++ b/src/mavsdk/plugins/camera_manager/camera_manager_impl.cpp @@ -18,19 +18,18 @@ class CameraManagerPluginImpl : public PluginImplBase { void enable() override; void disable() override; - void provide_camera_information(const std::function<(mavlink_camera_information_t& camera_information)>& callback) { - callback(camera_information); - camera_information_valid = true; + void provide_camera_information(const std::function<(mavlink_camera_information_t& +camera_information)>& callback) { callback(camera_information); camera_information_valid = true; } template - void add_param(const std::string& name, T default_value, const typename std::function& callback) { - _parent->provide_server_param_int("CAM_ISO", default_value); + void add_param(const std::string& name, T default_value, const typename std::function& callback) { _parent->provide_server_param_int("CAM_ISO", default_value); _parent->subscribe_param( name, overloaded { - [&name](auto value) { std::cerr << "invalid parameter type for " << name << std::endl; }, - callback + [&name](auto value) { std::cerr << "invalid parameter type for " << name << +std::endl; }, callback }, nullptr); } diff --git a/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h b/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h index cad4f09edd..e87838ce8f 100644 --- a/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h +++ b/src/mavsdk/plugins/camera_manager/include/plugins/camera_manager/camera_manager.h @@ -30,20 +30,25 @@ class CameraManagerPluginImpl : public PluginImplBase { void enable() override; void disable() override; - void provide_camera_information(const std::function<(mavlink_camera_information_t& camera_information)>& callback) { + void provide_camera_information( + const std::function<(mavlink_camera_information_t & camera_information)>& callback) + { callback(camera_information); camera_information_valid = true; } - template - void add_param(const std::string& name, T default_value, const typename std::function& callback) { + template + void add_param( + const std::string& name, T default_value, const typename std::function& callback) + { _parent->provide_server_param_int("CAM_ISO", default_value); _parent->subscribe_param( name, - overloaded { - [&name](auto value) { std::cerr << "invalid parameter type for " << name << std::endl; }, - callback - }, + overloaded{ + [&name](auto value) { + std::cerr << "invalid parameter type for " << name << std::endl; + }, + callback}, nullptr); } @@ -52,7 +57,8 @@ class CameraManagerPluginImpl : public PluginImplBase { bool camera_information_valid{false}; }; -void CameraManagerPluginImpl::init() { +void CameraManagerPluginImpl::init() +{ _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_COMMAND_LONG, [this](const mavlink_message_t& message) { @@ -80,4 +86,4 @@ void CameraManagerPluginImpl::deinit() {} void CameraManagerPluginImpl::enable() {} void CameraManagerPluginImpl::disable() {} -} +} // namespace mavsdk