zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
地面站终端 App
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
17643 Commits
2 Branches
0 Tags
363 MiB
Tree: 2076592ef7
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '2076592ef7'
${ noResults }
QG4/src/comm/MockLink.h

302 lines
16 KiB
Raw Blame History

/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include <QElapsedTimer>
#include <QMap>
#include <QLoggingCategory>
#include <QGeoCoordinate>
#include "MockLinkMissionItemHandler.h"
#include "MockLinkFTP.h"
#include "QGCMAVLink.h"
Q_DECLARE_LOGGING_CATEGORY(MockLinkLog)
Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog)
class LinkManager;
class MockConfiguration : public LinkConfiguration
{
Q_OBJECT
public:
MockConfiguration(const QString& name);
MockConfiguration(MockConfiguration* source);
Q_PROPERTY(int firmware READ firmware WRITE setFirmware NOTIFY firmwareChanged)
Q_PROPERTY(int vehicle READ vehicle WRITE setVehicle NOTIFY vehicleChanged)
Q_PROPERTY(bool sendStatus READ sendStatusText WRITE setSendStatusText NOTIFY sendStatusChanged)
Q_PROPERTY(bool incrementVehicleId READ incrementVehicleId WRITE setIncrementVehicleId NOTIFY incrementVehicleIdChanged)
int firmware (void) { return (int)_firmwareType; }
void setFirmware (int type) { _firmwareType = (MAV_AUTOPILOT)type; emit firmwareChanged(); }
int vehicle (void) { return (int)_vehicleType; }
bool incrementVehicleId (void) const { return _incrementVehicleId; }
void setVehicle (int type) { _vehicleType = (MAV_TYPE)type; emit vehicleChanged(); }
void setIncrementVehicleId (bool incrementVehicleId) { _incrementVehicleId = incrementVehicleId; emit incrementVehicleIdChanged(); }
MAV_AUTOPILOT firmwareType (void) { return _firmwareType; }
MAV_TYPE vehicleType (void) { return _vehicleType; }
bool sendStatusText (void) const { return _sendStatusText; }
void setFirmwareType (MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); }
void setVehicleType (MAV_TYPE vehicleType) { _vehicleType = vehicleType; emit vehicleChanged(); }
void setSendStatusText (bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); }
typedef enum {
FailNone, // No failures
FailParamNoReponseToRequestList, // Do no respond to PARAM_REQUEST_LIST
FailMissingParamOnInitialReqest, // Not all params are sent on initial request, should still succeed since QGC will re-query missing params
FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well
} FailureMode_t;
FailureMode_t failureMode(void) { return _failureMode; }
void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; }
// Overrides from LinkConfiguration
LinkType type (void) override { return LinkConfiguration::TypeMock; }
void copyFrom (LinkConfiguration* source) override;
void loadSettings (QSettings& settings, const QString& root) override;
void saveSettings (QSettings& settings, const QString& root) override;
QString settingsURL (void) override { return "MockLinkSettings.qml"; }
QString settingsTitle (void) override { return tr("Mock Link Settings"); }
signals:
void firmwareChanged (void);
void vehicleChanged (void);
void sendStatusChanged (void);
void incrementVehicleIdChanged (void);
private:
MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4;
MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR;
bool _sendStatusText = false;
FailureMode_t _failureMode = FailNone;
bool _incrementVehicleId = true;
static const char* _firmwareTypeKey;
static const char* _vehicleTypeKey;
static const char* _sendStatusTextKey;
static const char* _incrementVehicleIdKey;
static const char* _failureModeKey;
};
class MockLink : public LinkInterface
{
Q_OBJECT
public:
MockLink(SharedLinkConfigurationPtr& config);
virtual ~MockLink();
int vehicleId (void) const { return _vehicleSystemId; }
MAV_AUTOPILOT getFirmwareType (void) { return _firmwareType; }
void setFirmwareType (MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; }
void setSendStatusText (bool sendStatusText) { _sendStatusText = sendStatusText; }
void setFailureMode (MockConfiguration::FailureMode_t failureMode) { _failureMode = failureMode; }
/// APM stack has strange handling of the first item of the mission list. If it has no
/// onboard mission items, sometimes it sends back a home position in position 0 and
/// sometimes it doesn't. Don't ask. This option allows you to configure that behavior
/// for unit testing.
void setAPMMissionResponseMode(bool sendHomePositionOnEmptyList) { _apmSendHomePositionOnEmptyList = sendHomePositionOnEmptyList; }
void emitRemoteControlChannelRawChanged(int channel, uint16_t raw);
/// Sends the specified mavlink message to QGC
void respondWithMavlinkMessage(const mavlink_message_t& msg);
MockLinkFTP* mockLinkFTP(void) { return _mockLinkFTP; }
// Overrides from LinkInterface
bool isConnected(void) const override { return _connected; }
void disconnect (void) override;
/// Sets a failure mode for unit testingqgcm
/// @param failureMode Type of failure to simulate
/// @param failureAckResult Error to send if one the ack error modes
void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
void sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult);
/// Reset the state of the MissionItemHandler to no items, no transactions in progress.
void resetMissionItemHandler(void) { _missionItemHandler.reset(); }
/// Returns the filename for the simulated log file. Only available after a download is requested.
QString logDownloadFile(void) { return _logDownloadFilename; }
Q_INVOKABLE void setCommLost (bool commLost) { _commLost = commLost; }
Q_INVOKABLE void simulateConnectionRemoved (void);
static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startGenericMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startNoInitialConnectMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
// Special commands for testing COMMAND_LONG handlers. By default all commands except for MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY should retry.
static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 1);
void clearSendMavCommandCounts(void) { _sendMavCommandCountMap.clear(); }
int sendMavCommandCount(MAV_CMD command) { return _sendMavCommandCountMap[command]; }
// Special message ids for testing requestMessage support
typedef enum {
FailRequestMessageNone,
FailRequestMessageCommandAcceptedMsgNotSent,
FailRequestMessageCommandUnsupported,
FailRequestMessageCommandNoResponse,
FailRequestMessageCommandAcceptedSecondAttempMsgSent,
} RequestMessageFailureMode_t;
void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; }
signals:
void writeBytesQueuedSignal (const QByteArray bytes);
void highLatencyTransmissionEnabledChanged (bool highLatencyTransmissionEnabled);
private slots:
// LinkInterface overrides
void _writeBytes(const QByteArray bytes) final;
void _writeBytesQueued(const QByteArray bytes);
void _run1HzTasks(void);
void _run10HzTasks(void);
void _run500HzTasks(void);
private:
// LinkInterface overrides
bool _connect(void) override;
// QThread override
void run(void) final;
// MockLink methods
void _sendHeartBeat (void);
void _sendHighLatency2 (void);
void _handleIncomingNSHBytes (const char* bytes, int cBytes);
void _handleIncomingMavlinkBytes (const uint8_t* bytes, int cBytes);
void _loadParams (void);
void _handleHeartBeat (const mavlink_message_t& msg);
void _handleSetMode (const mavlink_message_t& msg);
void _handleParamRequestList (const mavlink_message_t& msg);
void _handleParamSet (const mavlink_message_t& msg);
void _handleParamRequestRead (const mavlink_message_t& msg);
void _handleFTP (const mavlink_message_t& msg);
void _handleCommandLong (const mavlink_message_t& msg);
void _handleManualControl (const mavlink_message_t& msg);
void _handlePreFlightCalibration (const mavlink_command_long_t& request);
void _handleLogRequestList (const mavlink_message_t& msg);
void _handleLogRequestData (const mavlink_message_t& msg);
void _handleParamMapRC (const mavlink_message_t& msg);
bool _handleRequestMessage (const mavlink_command_long_t& request, bool& noAck);
float _floatUnionForParam (int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat);
void _sendHomePosition (void);
void _sendGpsRawInt (void);
void _sendVibration (void);
void _sendSysStatus (void);
void _sendBatteryStatus (void);
void _sendStatusTextMessages (void);
void _sendChunkedStatusText (uint16_t chunkId, bool missingChunks);
void _respondWithAutopilotVersion (void);
void _sendRCChannels (void);
void _paramRequestListWorker (void);
void _logDownloadWorker (void);
void _sendADSBVehicles (void);
void _moveADSBVehicle (void);
void _sendGeneralMetaData (void);
static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
static MockLink* _startMockLink(MockConfiguration* mockConfig);
MockLinkMissionItemHandler _missionItemHandler;
QString _name;
bool _connected;
int _mavlinkChannel;
uint8_t _vehicleSystemId;
uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1;
bool _inNSH;
bool _mavlinkStarted;
uint8_t _mavBaseMode;
uint32_t _mavCustomMode;
uint8_t _mavState;
QElapsedTimer _runningTime;
static const int32_t _batteryMaxTimeRemaining = 15 * 60;
int8_t _battery1PctRemaining = 100;
int32_t _battery1TimeRemaining = _batteryMaxTimeRemaining;
MAV_BATTERY_CHARGE_STATE _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
int8_t _battery2PctRemaining = 100;
int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining;
MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
MAV_AUTOPILOT _firmwareType;
MAV_TYPE _vehicleType;
double _vehicleLatitude;
double _vehicleLongitude;
double _vehicleAltitude;
bool _commLost = false;
bool _highLatencyTransmissionEnabled = true;
MockLinkFTP* _mockLinkFTP = nullptr;
bool _sendStatusText;
bool _apmSendHomePositionOnEmptyList;
MockConfiguration::FailureMode_t _failureMode;
int _sendHomePositionDelayCount;
int _sendGPSPositionDelayCount;
int _currentParamRequestListComponentIndex; // Current component index for param request list workflow, -1 for no request in progress
int _currentParamRequestListParamIndex; // Current parameter index for param request list workflow
static const uint16_t _logDownloadLogId = 0; ///< Id of siumulated log file
static const uint32_t _logDownloadFileSize = 1000; ///< Size of simulated log file
QString _logDownloadFilename; ///< Filename for log download which is in progress
uint32_t _logDownloadCurrentOffset; ///< Current offset we are sending from
uint32_t _logDownloadBytesRemaining; ///< Number of bytes still to send, 0 = send inactive
QGeoCoordinate _adsbVehicleCoordinate;
double _adsbAngle;
RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
QMap<MAV_CMD, int> _sendMavCommandCountMap;
QMap<int, QMap<QString, QVariant>> _mapParamName2Value;
QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType;
static double _defaultVehicleLatitude;
static double _defaultVehicleLongitude;
static double _defaultVehicleAltitude;
static int _nextVehicleSystemId;
static const char* _failParam;
};