diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 35becd937c..712d71874f 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -173,6 +173,21 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t goto out; } + // check the sequence number: if this is a resent request, resend the last response + if (_last_reply_valid) { + mavlink_file_transfer_protocol_t *last_reply = reinterpret_cast(_last_reply); + PayloadHeader *last_payload = reinterpret_cast(&last_reply->payload[0]); + + if (payload->seq_number + 1 == last_payload->seq_number) { + // this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS + // resent the request + mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply); + return; + } + } + + + #ifdef MAVLINK_FTP_DEBUG printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset); @@ -268,12 +283,15 @@ out: payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { payload->size = 2; payload->data[1] = r_errno; } } + _last_reply_valid = false; + // Stream download replies are sent through mavlink stream mechanism. Unless we need to Nack. if (!stream_send || errorCode != kErrNone) { // respond to the request @@ -302,8 +320,18 @@ void MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) { -#ifdef MAVLINK_FTP_DEBUG PayloadHeader *payload = reinterpret_cast(&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, + // we can simply resend the response. + // we only keep small responses to reduce RAM usage and avoid large memcpy's. The larger responses are all data + // retrievals without side-effects, meaning it's ok to reexecute them if a response gets lost + if (payload->size <= sizeof(uint32_t)) { + _last_reply_valid = true; + memcpy(_last_reply, ftp_req, sizeof(_last_reply)); + } + +#ifdef MAVLINK_FTP_DEBUG warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number); #endif diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index a3b7c5d7be..eb57dddc77 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -199,6 +199,10 @@ private: #endif static constexpr const int _root_dir_len = sizeof(_root_dir) - 1; + bool _last_reply_valid = false; + uint8_t _last_reply[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN - MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + + sizeof(PayloadHeader) + sizeof(uint32_t)]; + // Mavlink test needs to be able to call send friend class MavlinkFtpTest; };