Don Gagne
11 years ago
5 changed files with 227 additions and 0 deletions
@ -0,0 +1,87 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#include "MockMavlinkFileServer.h" |
||||||
|
|
||||||
|
void MockMavlinkFileServer::sendMessage(mavlink_message_t message) |
||||||
|
{ |
||||||
|
Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); |
||||||
|
|
||||||
|
mavlink_encapsulated_data_t data; |
||||||
|
mavlink_msg_encapsulated_data_decode(&message, &data); |
||||||
|
const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; |
||||||
|
|
||||||
|
// FIXME: Check CRC
|
||||||
|
|
||||||
|
switch (hdr->opcode) { |
||||||
|
case kCmdNone: |
||||||
|
// ignored, always acked
|
||||||
|
|
||||||
|
RequestHeader ackHdr; |
||||||
|
ackHdr.magic = 'f'; |
||||||
|
ackHdr.opcode = kRspAck; |
||||||
|
ackHdr.session = 0; |
||||||
|
ackHdr.crc32 = 0; |
||||||
|
ackHdr.size = 0; |
||||||
|
// FIXME: Add CRC
|
||||||
|
//ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0);
|
||||||
|
|
||||||
|
mavlink_message_t ackMessage; |
||||||
|
mavlink_msg_encapsulated_data_pack(250, 0, &ackMessage, 0 /*_encdata_seq*/, (uint8_t*)&ackHdr); |
||||||
|
emit messageReceived(NULL, ackMessage); |
||||||
|
break; |
||||||
|
|
||||||
|
case kCmdTerminate: |
||||||
|
// releases sessionID, closes file
|
||||||
|
case kCmdReset: |
||||||
|
// terminates all sessions
|
||||||
|
case kCmdList: |
||||||
|
// list files in <path> from <offset>
|
||||||
|
case kCmdOpen: |
||||||
|
// opens <path> for reading, returns <session>
|
||||||
|
case kCmdRead: |
||||||
|
// reads <size> bytes from <offset> in <session>
|
||||||
|
case kCmdCreate: |
||||||
|
// creates <path> for writing, returns <session>
|
||||||
|
case kCmdWrite: |
||||||
|
// appends <size> bytes at <offset> in <session>
|
||||||
|
case kCmdRemove: |
||||||
|
// remove file (only if created by server?)
|
||||||
|
default: |
||||||
|
// nack for all NYI opcodes
|
||||||
|
|
||||||
|
RequestHeader nakHdr; |
||||||
|
nakHdr.opcode = kRspNak; |
||||||
|
nakHdr.magic = 'f'; |
||||||
|
nakHdr.session = 0; |
||||||
|
nakHdr.crc32 = 0; |
||||||
|
nakHdr.size = 0; |
||||||
|
// FIXME: Add CRC
|
||||||
|
//ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0);
|
||||||
|
|
||||||
|
mavlink_message_t nakMessage; |
||||||
|
mavlink_msg_encapsulated_data_pack(250, 0, &nakMessage, 0 /*_encdata_seq*/, (uint8_t*)&nakHdr); |
||||||
|
emit messageReceived(NULL, nakMessage); |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,85 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#ifndef MOCKMAVLINKFILESERVER_H |
||||||
|
#define MOCKMAVLINKFILESERVER_H |
||||||
|
|
||||||
|
#include "MockMavlinkInterface.h" |
||||||
|
|
||||||
|
class MockMavlinkFileServer : public MockMavlinkInterface |
||||||
|
{ |
||||||
|
Q_OBJECT |
||||||
|
|
||||||
|
public: |
||||||
|
MockMavlinkFileServer(void) { }; |
||||||
|
virtual void sendMessage(mavlink_message_t message); |
||||||
|
|
||||||
|
private: |
||||||
|
// FIXME: These should be in a mavlink header somewhere shouldn't they?
|
||||||
|
|
||||||
|
struct RequestHeader |
||||||
|
{ |
||||||
|
uint8_t magic; |
||||||
|
uint8_t session; |
||||||
|
uint8_t opcode; |
||||||
|
uint8_t size; |
||||||
|
uint32_t crc32; |
||||||
|
uint32_t offset; |
||||||
|
uint8_t data[]; |
||||||
|
}; |
||||||
|
|
||||||
|
enum Opcode |
||||||
|
{ |
||||||
|
kCmdNone, // ignored, always acked
|
||||||
|
kCmdTerminate, // releases sessionID, closes file
|
||||||
|
kCmdReset, // terminates all sessions
|
||||||
|
kCmdList, // list files in <path> from <offset>
|
||||||
|
kCmdOpen, // opens <path> for reading, returns <session>
|
||||||
|
kCmdRead, // reads <size> bytes from <offset> in <session>
|
||||||
|
kCmdCreate, // creates <path> for writing, returns <session>
|
||||||
|
kCmdWrite, // appends <size> bytes at <offset> in <session>
|
||||||
|
kCmdRemove, // remove file (only if created by server?)
|
||||||
|
|
||||||
|
kRspAck, |
||||||
|
kRspNak |
||||||
|
}; |
||||||
|
|
||||||
|
enum ErrorCode |
||||||
|
{ |
||||||
|
kErrNone, |
||||||
|
kErrNoRequest, |
||||||
|
kErrNoSession, |
||||||
|
kErrSequence, |
||||||
|
kErrNotDir, |
||||||
|
kErrNotFile, |
||||||
|
kErrEOF, |
||||||
|
kErrNotAppend, |
||||||
|
kErrTooBig, |
||||||
|
kErrIO, |
||||||
|
kErrPerm |
||||||
|
}; |
||||||
|
|
||||||
|
|
||||||
|
}; |
||||||
|
|
||||||
|
#endif |
||||||
@ -0,0 +1,9 @@ |
|||||||
|
//
|
||||||
|
// MockMavlinkInterface.cc
|
||||||
|
// QGroundControl
|
||||||
|
//
|
||||||
|
// Created by Donald Gagne on 6/19/14.
|
||||||
|
// Copyright (c) 2014 Donald Gagne. All rights reserved.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "MockMavlinkInterface.h" |
||||||
@ -0,0 +1,43 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#include <QObject> |
||||||
|
|
||||||
|
#include "QGCMAVLink.h" |
||||||
|
#include "LinkInterface.h" |
||||||
|
|
||||||
|
#ifndef MOCKMAVLINKINTERFACE_H |
||||||
|
#define MOCKMAVLINKINTERFACE_H |
||||||
|
|
||||||
|
class MockMavlinkInterface : public QObject |
||||||
|
{ |
||||||
|
Q_OBJECT |
||||||
|
|
||||||
|
public: |
||||||
|
virtual void sendMessage(mavlink_message_t message) = 0; |
||||||
|
|
||||||
|
signals: |
||||||
|
void messageReceived(LinkInterface* link, mavlink_message_t message); |
||||||
|
}; |
||||||
|
|
||||||
|
#endif |
||||||
Loading…
Reference in new issue