fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink FTP: Fix for wrong component id in retry

Browse Source
This commit is contained in:
Matej Frančeškin 2019-07-31 20:43:29 +02:00 committed by Beat Küng
parent 43d006aff2
commit 3d993773ae
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -138,14 +138,17 @@ MavlinkFTP::handle_message(const mavlink_message_t *msg)
if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) &&
(ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 0)) {
_process_request(&ftp_request, msg->sysid);
_process_request(&ftp_request, msg->sysid, msg->compid);
}
}
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id)
MavlinkFTP::_process_request(
mavlink_file_transfer_protocol_t *ftp_req,
uint8_t target_system_id,
uint8_t target_comp_id)
{
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
@ -218,7 +221,7 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
errorCode = _workBurst(payload, target_system_id, target_comp_id);
stream_send = true;
break;
@ -287,6 +290,8 @@ out:
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
ftp_req->target_network = 0;
ftp_req->target_component = target_comp_id;
_reply(ftp_req);
}
}
@ -310,7 +315,6 @@ bool MavlinkFTP::_ensure_buffers_exist()
void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
// keep a copy of the last sent response ((n)ack), so that if it gets lost and the GCS resends the request,
@ -326,8 +330,6 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
PX4_INFO("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data);
@ -575,7 +577,7 @@ MavlinkFTP::_workRead(PayloadHeader *payload)
/// @brief Responds to a Stream command
MavlinkFTP::ErrorCode
MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id)
MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
@ -590,6 +592,7 @@ MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id)
_session_info.stream_chunk_transmitted = 0;
_session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id;
_session_info.stream_target_component_id = target_component_id;
return kErrNone;
}
@ -1079,6 +1082,8 @@ void MavlinkFTP::send(const hrt_abstime t)
}
ftp_msg.target_system = _session_info.stream_target_system_id;
ftp_msg.target_network = 0;
ftp_msg.target_component = _session_info.stream_target_component_id;
_reply(&ftp_msg);
} while (more_data);
}

5
src/modules/mavlink/mavlink_ftp.h
View File

@ -128,14 +128,14 @@ public:
private:
char *_data_as_cstring(PayloadHeader *payload);
void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id);
void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id, uint8_t target_comp_id);
void _reply(mavlink_file_transfer_protocol_t *ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id);
ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader *payload);
@ -170,6 +170,7 @@ private:
uint32_t stream_offset;
uint16_t stream_seq_number;
uint8_t stream_target_system_id;
uint8_t stream_target_component_id;
unsigned stream_chunk_transmitted;
};
struct SessionInfo _session_info {}; ///< Session info, fd=-1 for no active session