Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ftp.cpp
+12 -7
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) && if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) &&
(ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 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 /// @brief Processes an FTP message
void 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; bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]); 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; break;
case kCmdBurstReadFile: case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id); errorCode = _workBurst(payload, target_system_id, target_comp_id);
stream_send = true; stream_send = true;
break; break;
@@ -287,6 +290,8 @@ out:
if (!stream_send || errorCode != kErrNone) { if (!stream_send || errorCode != kErrNone) {
// respond to the request // respond to the request
ftp_req->target_system = target_system_id; ftp_req->target_system = target_system_id;
ftp_req->target_network = 0;
ftp_req->target_component = target_comp_id;
_reply(ftp_req); _reply(ftp_req);
} }
} }
@@ -310,7 +315,6 @@ bool MavlinkFTP::_ensure_buffers_exist()
void void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
{ {
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]); 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, // 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); PX4_INFO("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif #endif
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST #ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead // Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data); _utRcvMsgFunc(ftp_req, _worker_data);
@@ -575,7 +577,7 @@ MavlinkFTP::_workRead(PayloadHeader *payload)
/// @brief Responds to a Stream command /// @brief Responds to a Stream command
MavlinkFTP::ErrorCode 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) { if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession; return kErrInvalidSession;
@@ -590,6 +592,7 @@ MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id)
_session_info.stream_chunk_transmitted = 0; _session_info.stream_chunk_transmitted = 0;
_session_info.stream_seq_number = payload->seq_number + 1; _session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id; _session_info.stream_target_system_id = target_system_id;
_session_info.stream_target_component_id = target_component_id;
return kErrNone; 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_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); _reply(&ftp_msg);
} while (more_data); } while (more_data);
} }
src/modules/mavlink/mavlink_ftp.h
+3 -2
View File
@@ -128,14 +128,14 @@ public:
private: private:
char *_data_as_cstring(PayloadHeader *payload); 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); void _reply(mavlink_file_transfer_protocol_t *ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length); int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload); 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 _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader *payload); ErrorCode _workReset(PayloadHeader *payload);
@@ -170,6 +170,7 @@ private:
uint32_t stream_offset; uint32_t stream_offset;
uint16_t stream_seq_number; uint16_t stream_seq_number;
uint8_t stream_target_system_id; uint8_t stream_target_system_id;
uint8_t stream_target_component_id;
unsigned stream_chunk_transmitted; unsigned stream_chunk_transmitted;
}; };
struct SessionInfo _session_info {}; ///< Session info, fd=-1 for no active session struct SessionInfo _session_info {}; ///< Session info, fd=-1 for no active session