diff options
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.cpp | 13 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.h | 25 |
2 files changed, 21 insertions, 17 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 7a72835cc..d4d659d91 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -73,10 +73,13 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) if (req != nullptr) { // decode the request - req->decode(channel, msg); + if (req->decode(msg, channel)) { - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } else { + _qFree(req); + } } } @@ -112,7 +115,7 @@ MavlinkFTP::_worker(Request *req) printf("ftp: bad crc\n"); } - printf("ftp: opc %u size %u offset %u\n", hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); switch (hdr->opcode) { case kCmdNone: @@ -186,7 +189,7 @@ MavlinkFTP::_reply(Request *req) hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source - mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->rawData()); + req->reply(); } MavlinkFTP::ErrorCode diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 6a2414613..f68dab98d 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -144,19 +144,18 @@ private: dq_entry_t entry; work_s work; }; - mavlink_channel_t channel; - - void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) { - switch (fromMessage->msgid) { - - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - channel = fromChannel; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - warnx("got enc data"); - break; - default: - warnx("unknown msg"); + + bool decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) { + if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + _channel = fromChannel; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + return true; } + return false; + } + + void reply() { + mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData()); } uint8_t *rawData() { return &_message.data[0]; } @@ -164,10 +163,12 @@ private: uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } uint16_t sequence() const { return _message.seqnr; } + mavlink_channel_t &channel() { return _channel; } char *dataAsCString(); private: + mavlink_channel_t _channel; mavlink_encapsulated_data_t _message; }; |