Skip to content

Commit

Permalink
ftp plugin: handle burst operation
Browse files Browse the repository at this point in the history
  • Loading branch information
@dayjaby
dayjaby committed Dec 25, 2021
1 parent 96f6cac commit cc4522c
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 14 deletions.
6 changes: 2 additions & 4 deletions examples/camera_manager/camera_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ void usage(const std::string& bin_name)
{
std::cerr << "Usage : " << bin_name << " <root_dir>\n"
<< '\n'
<< " Start mavlink camera_manager with camera definition file in <root_dir>\n"
<< " Start mavlink camera_manager with camera definition file in <root_dir>" << std::endl;
}

int main(int argc, char** argv)
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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);
Expand Down
105 changes: 96 additions & 9 deletions src/mavsdk/plugins/ftp/ftp_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); },
Expand Down Expand Up @@ -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;
}

Expand All @@ -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) {
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
Expand Down Expand Up @@ -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<PayloadHeader *>(&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
4 changes: 3 additions & 1 deletion src/mavsdk/plugins/ftp/ftp_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
};

Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand Down

0 comments on commit cc4522c

Please sign in to comment.