void MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) { // get a free request struct Request* req = _get_request(); // if we couldn't get a request slot, just drop it if (req == nullptr) { warnx("Dropping FTP request: queue full\n"); return; } if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { mavlink_msg_file_transfer_protocol_decode(msg, &req->message); #ifdef MAVLINK_FTP_UNIT_TEST if (!_utRcvMsgFunc) { warnx("Incorrectly written unit test\n"); return; } // We use fake ids when unit testing req->serverSystemId = MavlinkFtpTest::serverSystemId; req->serverComponentId = MavlinkFtpTest::serverComponentId; req->serverChannel = MavlinkFtpTest::serverChannel; #else // Not unit testing, use the real thing req->serverSystemId = mavlink->get_system_id(); req->serverComponentId = mavlink->get_component_id(); req->serverChannel = mavlink->get_channel(); #endif // This is the system id we want to target when sending req->targetSystemId = msg->sysid; if (req->message.target_system == req->serverSystemId) { req->mavlink = mavlink; #ifdef MAVLINK_FTP_UNIT_TEST // We are running in Unit Test mode. Don't queue, just call _worket directly. _process_request(req); #else // We are running in normal mode. Queue the request to the worker work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); #endif return; } } _return_request(req); }
void MavlinkFTP::handle_message(const mavlink_message_t *msg) { //warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2); if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { mavlink_file_transfer_protocol_t ftp_request; mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request); #ifdef MAVLINK_FTP_DEBUG warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system); #endif if (ftp_request.target_system == _getServerSystemId()) { _process_request(&ftp_request, msg->sysid); } } }
/// @brief Decode and validate the incoming message bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response { mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); // Make sure the targets are correct ut_compare("Target network non-zero", ftp_msg->target_network, 0); ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload); // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); // Bump sequence number for next outgoing message _lastOutgoingSeqNumber++; return true; }