From 8eedfc49bf2172e4b90f681f71db7df3a458ffa4 Mon Sep 17 00:00:00 2001 From: Holden Date: Tue, 7 Jan 2025 04:51:44 -0500 Subject: [PATCH] Comms: Update Mocklink Threading --- src/Comms/MockLink/CMakeLists.txt | 2 + src/Comms/MockLink/MockConfiguration.cc | 76 ++ src/Comms/MockLink/MockConfiguration.h | 88 ++ src/Comms/MockLink/MockLink.cc | 1326 +++++++++++------------ src/Comms/MockLink/MockLink.h | 406 +++---- 5 files changed, 947 insertions(+), 951 deletions(-) create mode 100644 src/Comms/MockLink/MockConfiguration.cc create mode 100644 src/Comms/MockLink/MockConfiguration.h diff --git a/src/Comms/MockLink/CMakeLists.txt b/src/Comms/MockLink/CMakeLists.txt index b5902cde9407..6c845453718f 100644 --- a/src/Comms/MockLink/CMakeLists.txt +++ b/src/Comms/MockLink/CMakeLists.txt @@ -7,6 +7,8 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug") target_sources(MockLink PRIVATE + MockConfiguration.cc + MockConfiguration.h MockLink.cc MockLink.h MockLinkFTP.cc diff --git a/src/Comms/MockLink/MockConfiguration.cc b/src/Comms/MockLink/MockConfiguration.cc new file mode 100644 index 000000000000..926354719c07 --- /dev/null +++ b/src/Comms/MockLink/MockConfiguration.cc @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "MockConfiguration.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(MockConfigurationLog, "qgc.comms.mocklink.mockconfiguration") + +MockConfiguration::MockConfiguration(const QString &name, QObject *parent) + : LinkConfiguration(name, parent) +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +MockConfiguration::MockConfiguration(const MockConfiguration *copy, QObject *parent) + : LinkConfiguration(copy, parent) + , _firmwareType(copy->firmwareType()) + , _vehicleType(copy->vehicleType()) + , _sendStatusText(copy->sendStatusText()) + , _incrementVehicleId(copy->incrementVehicleId()) + , _failureMode(copy->failureMode()) +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +MockConfiguration::~MockConfiguration() +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +void MockConfiguration::copyFrom(const LinkConfiguration *source) +{ + Q_ASSERT(source); + LinkConfiguration::copyFrom(source); + + const MockConfiguration* const mockLinkSource = qobject_cast(source); + Q_ASSERT(mockLinkSource); + + setFirmwareType(mockLinkSource->firmwareType()); + setVehicleType(mockLinkSource->vehicleType()); + setSendStatusText(mockLinkSource->sendStatusText()); + setIncrementVehicleId(mockLinkSource->incrementVehicleId()); + setFailureMode(mockLinkSource->failureMode()); +} + +void MockConfiguration::loadSettings(QSettings &settings, const QString &root) +{ + settings.beginGroup(root); + + setFirmwareType(static_cast(settings.value(_firmwareTypeKey, static_cast(MAV_AUTOPILOT_PX4)).toInt())); + setVehicleType(static_cast(settings.value(_vehicleTypeKey, static_cast(MAV_TYPE_QUADROTOR)).toInt())); + setSendStatusText(settings.value(_sendStatusTextKey, false).toBool()); + setIncrementVehicleId(settings.value(_incrementVehicleIdKey, true).toBool()); + setFailureMode(static_cast(settings.value(_failureModeKey, static_cast(FailNone)).toInt())); + + settings.endGroup(); +} + +void MockConfiguration::saveSettings(QSettings &settings, const QString &root) +{ + settings.beginGroup(root); + + settings.setValue(_firmwareTypeKey, firmwareType()); + settings.setValue(_vehicleTypeKey, vehicleType()); + settings.setValue(_sendStatusTextKey, sendStatusText()); + settings.setValue(_incrementVehicleIdKey, incrementVehicleId()); + settings.setValue(_failureModeKey, failureMode()); + + settings.endGroup(); +} diff --git a/src/Comms/MockLink/MockConfiguration.h b/src/Comms/MockLink/MockConfiguration.h new file mode 100644 index 000000000000..51df7b74bdeb --- /dev/null +++ b/src/Comms/MockLink/MockConfiguration.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "LinkConfiguration.h" +#include "MAVLinkLib.h" + +#include + +Q_DECLARE_LOGGING_CATEGORY(MockConfigurationLog) + +class MockConfiguration : public LinkConfiguration +{ + Q_OBJECT + 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) + +public: + explicit MockConfiguration(const QString &name, QObject *parent = nullptr); + explicit MockConfiguration(const MockConfiguration *copy, QObject *parent = nullptr); + ~MockConfiguration(); + + LinkType type() const final { return LinkConfiguration::TypeMock; } + void copyFrom(const LinkConfiguration *source) final; + void loadSettings(QSettings &settings, const QString &root) final; + void saveSettings(QSettings &settings, const QString &root) final; + QString settingsURL() final { return QStringLiteral("MockLinkSettings.qml"); } + QString settingsTitle() final { return tr("Mock Link Settings"); } + + int firmware() const { return static_cast(_firmwareType); } + void setFirmware(int type) { _firmwareType = static_cast(type); emit firmwareChanged(); } + int vehicle() const { return static_cast(_vehicleType); } + void setVehicle(int type) { _vehicleType = static_cast(type); emit vehicleChanged(); } + bool incrementVehicleId() const { return _incrementVehicleId; } + void setIncrementVehicleId(bool incrementVehicleId) { _incrementVehicleId = incrementVehicleId; emit incrementVehicleIdChanged(); } + MAV_AUTOPILOT firmwareType() const { return _firmwareType; } + void setFirmwareType(MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); } + uint16_t boardVendorId() const { return _boardVendorId; } + uint16_t boardProductId() const { return _boardProductId; } + void setBoardVendorProduct(uint16_t vendorId, uint16_t productId) { _boardVendorId = vendorId; _boardProductId = productId; } + MAV_TYPE vehicleType() const { return _vehicleType; } + void setVehicleType(MAV_TYPE vehicleType) { _vehicleType = vehicleType; emit vehicleChanged(); } + bool sendStatusText() const { return _sendStatusText; } + void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); } + + enum FailureMode_t { + 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 + FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure + FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent + FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure + FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent + }; + FailureMode_t failureMode() const { return _failureMode; } + void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } + +signals: + void firmwareChanged(); + void vehicleChanged(); + void sendStatusChanged(); + void incrementVehicleIdChanged(); + +private: + MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; + MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; + bool _sendStatusText = false; + FailureMode_t _failureMode = FailNone; + bool _incrementVehicleId = true; + uint16_t _boardVendorId = 0; + uint16_t _boardProductId = 0; + + static constexpr const char *_firmwareTypeKey = "FirmwareType"; + static constexpr const char *_vehicleTypeKey = "VehicleType"; + static constexpr const char *_sendStatusTextKey = "SendStatusText"; + static constexpr const char *_incrementVehicleIdKey = "IncrementVehicleId"; + static constexpr const char *_failureModeKey = "FailureMode"; +}; diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index 2b7a2822c5d4..99fac1f5a537 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -8,98 +8,61 @@ ****************************************************************************/ #include "MockLink.h" -#include "QGCLoggingCategory.h" -#include "QGCApplication.h" + #include "LinkManager.h" #include "MAVLinkProtocol.h" +#include "QGCApplication.h" #include "QGCLoggingCategory.h" #include #include -#include -#include #include - -#include - -// FIXME: Hack to work around clean headers -#include "FirmwarePlugin/PX4/px4_custom_mode.h" +#include +#include QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog") QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") -// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle -// testing of a gazebo vehicle and a mocklink vehicle -#if 1 -double MockLink::_defaultVehicleLatitude = 47.397; -double MockLink::_defaultVehicleLongitude = 8.5455; -double MockLink::_defaultVehicleHomeAltitude = 488.056; -#else -double MockLink::_defaultVehicleLatitude = 47.6333022928789; -double MockLink::_defaultVehicleLongitude = -122.08833157994995; -double MockLink::_defaultVehicleHomeAltitude = 19.0; -#endif -int MockLink::_nextVehicleSystemId = 128; +int MockLink::_nextVehicleSystemId = 128; // The LinkManager is only forward declared in the header, so a static_assert is here instead to ensure we update if the value changes. static_assert(LinkManager::invalidMavlinkChannel() == std::numeric_limits::max(), "update MockLink::_mavlinkAuxChannel"); -MockLink::MockLink(SharedLinkConfigurationPtr& config) - : LinkInterface (config) - , _missionItemHandler (this, MAVLinkProtocol::instance()) - , _name ("MockLink") - , _connected (false) - , _vehicleComponentId (MAV_COMP_ID_AUTOPILOT1) - , _inNSH (false) - , _mavlinkStarted (true) - , _mavBaseMode (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) - , _mavState (MAV_STATE_STANDBY) - , _firmwareType (MAV_AUTOPILOT_PX4) - , _vehicleType (MAV_TYPE_QUADROTOR) - , _vehicleAltitudeAMSL (_defaultVehicleHomeAltitude) - , _sendStatusText (false) - , _apmSendHomePositionOnEmptyList (false) - , _failureMode (MockConfiguration::FailNone) - , _sendHomePositionDelayCount (10) // No home position for 4 seconds - , _sendGPSPositionDelayCount (100) // No gps lock for 5 seconds - , _currentParamRequestListComponentIndex(-1) - , _currentParamRequestListParamIndex (-1) - , _logDownloadCurrentOffset (0) - , _logDownloadBytesRemaining (0) - , _adsbAngles {0} -{ - qCDebug(MockLinkLog) << "MockLink" << this; +MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent) + : LinkInterface(config, parent) + , _missionItemHandler(new MockLinkMissionItemHandler(this, MAVLinkProtocol::instance())) + , _vehicleAltitudeAMSL(_defaultVehicleHomeAltitude) +{ + // qCDebug(MockLinkLog) << Q_FUNC_INFO << this; // Initialize 5 ADS-B vehicles with different starting conditions _numberOfVehicles - for (int i = 0; i < 5; ++i) { - ADSBVehicle vehicle; + for (int i = 0; i < _numberOfVehicles; ++i) { + ADSBVehicle vehicle{}; vehicle.angle = i * 72; // Different starting directions (angles 0, 72, 144, 216, 288) - + // Set initial coordinates slightly offset from the default coordinates - double latOffset = 0.001 * i; // Adjust latitude slightly for each vehicle (close proximity) - double lonOffset = 0.001 * (i % 2 == 0 ? i : -i); // Adjust longitude with a pattern (close proximity) + const double latOffset = 0.001 * i; // Adjust latitude slightly for each vehicle (close proximity) + const double lonOffset = 0.001 * (i % 2 == 0 ? i : -i); // Adjust longitude with a pattern (close proximity) vehicle.coordinate = QGeoCoordinate(_defaultVehicleLatitude + latOffset, _defaultVehicleLongitude + lonOffset); - + // Set a unique starting altitude for each vehicle near the home altitude vehicle.altitude = _defaultVehicleHomeAltitude + (i * 5); // Altitudes: close to default, with slight variation - + _adsbVehicles.push_back(vehicle); } - - MockConfiguration* mockConfig = qobject_cast(_config.get()); - _firmwareType = mockConfig->firmwareType(); - _vehicleType = mockConfig->vehicleType(); - _sendStatusText = mockConfig->sendStatusText(); - _failureMode = mockConfig->failureMode(); - _vehicleSystemId = mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : _nextVehicleSystemId; - _vehicleLatitude = _defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001); - _vehicleLongitude = _defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001); - _boardVendorId = mockConfig->boardVendorId(); - _boardProductId = mockConfig->boardProductId(); - - QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); - - _mavCustomMode = PX4CustomMode::MANUAL; + + MockConfiguration *const mockConfig = qobject_cast(_config.get()); + setFirmwareType(mockConfig->firmwareType()); + _vehicleType = mockConfig->vehicleType(); + setSendStatusText(mockConfig->sendStatusText()); + setFailureMode(mockConfig->failureMode()); + _vehicleSystemId = mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : _nextVehicleSystemId; + _vehicleLatitude = _defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001); + _vehicleLongitude = _defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001); + _boardVendorId = mockConfig->boardVendorId(); + _boardProductId = mockConfig->boardProductId(); + + (void) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); _mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this); @@ -109,16 +72,18 @@ MockLink::MockLink(SharedLinkConfigurationPtr& config) _runningTime.start(); } -MockLink::~MockLink(void) +MockLink::~MockLink() { - disconnect(); + MockLink::disconnect(); + if (!_logDownloadFilename.isEmpty()) { QFile::remove(_logDownloadFilename); } - qCDebug(MockLinkLog) << "~MockLink" << this; + + // qCDebug(MockLinkLog) << Q_FUNC_INFO << this; } -bool MockLink::_connect(void) +bool MockLink::_connect() { if (!_connected) { _connected = true; @@ -167,17 +132,12 @@ void MockLink::_freeMavlinkChannel() LinkInterface::_freeMavlinkChannel(); } -uint8_t MockLink::mavlinkAuxChannel(void) const -{ - return _mavlinkAuxChannel; -} - -bool MockLink::mavlinkAuxChannelIsSet(void) const +bool MockLink::mavlinkAuxChannelIsSet() const { return (LinkManager::invalidMavlinkChannel() != _mavlinkAuxChannel); } -void MockLink::disconnect(void) +void MockLink::disconnect() { if (_connected) { _connected = false; @@ -187,12 +147,12 @@ void MockLink::disconnect(void) } } -void MockLink::run(void) +void MockLink::run() { - QTimer timer1HzTasks; - QTimer timer10HzTasks; - QTimer timer500HzTasks; - QTimer timerStatusText; + QTimer timer1HzTasks; + QTimer timer10HzTasks; + QTimer timer500HzTasks; + QTimer timerStatusText; QObject::connect(&timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); @@ -220,10 +180,10 @@ void MockLink::run(void) QObject::disconnect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); QObject::disconnect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks); - _missionItemHandler.shutdown(); + _missionItemHandler->shutdown(); } -void MockLink::_run1HzTasks(void) +void MockLink::_run1HzTasks() { if (_mavlinkStarted && _connected) { if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) { @@ -248,7 +208,7 @@ void MockLink::_run1HzTasks(void) } } -void MockLink::_run10HzTasks(void) +void MockLink::_run10HzTasks() { if (linkConfiguration()->isHighLatency()) { return; @@ -267,7 +227,7 @@ void MockLink::_run10HzTasks(void) } } -void MockLink::_run500HzTasks(void) +void MockLink::_run500HzTasks() { if (linkConfiguration()->isHighLatency()) { return; @@ -279,10 +239,9 @@ void MockLink::_run500HzTasks(void) } } -void MockLink::_loadParams(void) +void MockLink::_loadParams() { QFile paramFile; - if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { if (_vehicleType == MAV_TYPE_FIXED_WING) { paramFile.setFileName(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); @@ -298,26 +257,25 @@ void MockLink::_loadParams(void) } - bool success = paramFile.open(QFile::ReadOnly); + const bool success = paramFile.open(QFile::ReadOnly); Q_UNUSED(success); Q_ASSERT(success); QTextStream paramStream(¶mFile); - while (!paramStream.atEnd()) { - QString line = paramStream.readLine(); + const QString line = paramStream.readLine(); if (line.startsWith("#")) { continue; } - QStringList paramData = line.split("\t"); + const QStringList paramData = line.split("\t"); Q_ASSERT(paramData.count() == 5); - int compId = paramData.at(1).toInt(); - QString paramName = paramData.at(2); - QString valStr = paramData.at(3); - uint paramType = paramData.at(4).toUInt(); + const int compId = paramData.at(1).toInt(); + const QString paramName = paramData.at(2); + const QString valStr = paramData.at(3); + const uint paramType = paramData.at(4).toUInt(); QVariant paramValue; switch (paramType) { @@ -355,85 +313,88 @@ void MockLink::_loadParams(void) } } -void MockLink::_sendHeartBeat(void) -{ - mavlink_message_t msg; - - mavlink_msg_heartbeat_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - _vehicleType, // MAV_TYPE - _firmwareType, // MAV_AUTOPILOT - _mavBaseMode, // MAV_MODE - _mavCustomMode, // custom mode - _mavState); // MAV_STATE - +void MockLink::_sendHeartBeat() +{ + mavlink_message_t msg{}; + mavlink_msg_heartbeat_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + _vehicleType, // MAV_TYPE + _firmwareType, // MAV_AUTOPILOT + _mavBaseMode, // MAV_MODE + _mavCustomMode, // custom mode + _mavState // MAV_STATE + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendHighLatency2(void) +void MockLink::_sendHighLatency2() { - mavlink_message_t msg; + qCDebug(MockLinkLog) << "Sending" << _mavCustomMode; - union px4_custom_mode px4_cm; + union px4_custom_mode px4_cm{}; px4_cm.data = _mavCustomMode; - qCDebug(MockLinkLog) << "Sending" << _mavCustomMode; - mavlink_msg_high_latency2_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - 0, // timestamp - _vehicleType, // MAV_TYPE - _firmwareType, // MAV_AUTOPILOT - px4_cm.custom_mode_hl, // custom_mode - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int16_t)_vehicleAltitudeAMSL, - (int16_t)_vehicleAltitudeAMSL, // target_altitude, - 0, // heading - 0, // target_heading - 0, // target_distance - 0, // throttle - 0, // airspeed - 0, // airspeed_sp - 0, // groundspeed - 0, // windspeed, - 0, // wind_heading - UINT8_MAX, // eph not known - UINT8_MAX, // epv not known - 0, // temperature_air - 0, // climb_rate - -1, // battery, do not use? - 0, // wp_num - 0, // failure_flags - 0, 0, 0); // custom0, custom1, custom2 + mavlink_message_t msg{}; + (void) mavlink_msg_high_latency2_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // timestamp + _vehicleType, // MAV_TYPE + _firmwareType, // MAV_AUTOPILOT + px4_cm.custom_mode_hl, // custom_mode + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int16_t)_vehicleAltitudeAMSL, + (int16_t)_vehicleAltitudeAMSL, // target_altitude, + 0, // heading + 0, // target_heading + 0, // target_distance + 0, // throttle + 0, // airspeed + 0, // airspeed_sp + 0, // groundspeed + 0, // windspeed, + 0, // wind_heading + UINT8_MAX, // eph not known + UINT8_MAX, // epv not known + 0, // temperature_air + 0, // climb_rate + -1, // battery, do not use? + 0, // wp_num + 0, // failure_flags + 0, 0, 0 // custom0, custom1, custom2 + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendSysStatus(void) -{ - mavlink_message_t msg; - mavlink_msg_sys_status_pack_chan( - _vehicleSystemId, - _vehicleComponentId, - static_cast(mavlinkChannel()), - &msg, - MAV_SYS_STATUS_SENSOR_GPS, // onboard_control_sensors_present - 0, // onboard_control_sensors_enabled - 0, // onboard_control_sensors_health - 250, // load - 4200 * 4, // voltage_battery - 8000, // current_battery - _battery1PctRemaining, // battery_remaining - 0,0,0,0,0,0,0,0,0); +void MockLink::_sendSysStatus() +{ + mavlink_message_t msg{}; + (void) mavlink_msg_sys_status_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + static_cast(mavlinkChannel()), + &msg, + MAV_SYS_STATUS_SENSOR_GPS, // onboard_control_sensors_present + 0, // onboard_control_sensors_enabled + 0, // onboard_control_sensors_health + 250, // load + 4200 * 4, // voltage_battery + 8000, // current_battery + _battery1PctRemaining, // battery_remaining + 0,0,0,0,0,0,0,0,0 + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendBatteryStatus(void) +void MockLink::_sendBatteryStatus() { - if(_battery1PctRemaining > 1) { + if (_battery1PctRemaining > 1) { _battery1PctRemaining = static_cast(100 - (_runningTime.elapsed() / 1000)); _battery1TimeRemaining = static_cast(_batteryMaxTimeRemaining) * (static_cast(_battery1PctRemaining) / 100.0); if (_battery1PctRemaining > 50) { @@ -446,7 +407,8 @@ void MockLink::_sendBatteryStatus(void) _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY; } } - if(_battery2PctRemaining > 1) { + + if (_battery2PctRemaining > 1) { _battery2PctRemaining = static_cast(100 - ((_runningTime.elapsed() / 1000) / 2)); _battery2TimeRemaining = static_cast(_batteryMaxTimeRemaining) * (static_cast(_battery2PctRemaining) / 100.0); if (_battery2PctRemaining > 50) { @@ -460,127 +422,124 @@ void MockLink::_sendBatteryStatus(void) } } - mavlink_message_t msg; - uint16_t rgVoltages[10]; - uint16_t rgVoltagesNone[10]; - uint16_t rgVoltagesExtNone[4]; + mavlink_message_t msg{}; + uint16_t rgVoltages[10]{}; + uint16_t rgVoltagesNone[10]{}; + uint16_t rgVoltagesExtNone[4]{}; - for (int i=0; i<10; i++) { - rgVoltages[i] = UINT16_MAX; - rgVoltagesNone[i] = UINT16_MAX; + for (int i = 0; i < std::size(rgVoltages); i++) { + rgVoltages[i] = UINT16_MAX; + rgVoltagesNone[i] = UINT16_MAX; } rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200; - memset(rgVoltagesExtNone, 0, sizeof(rgVoltagesExtNone)); - - mavlink_msg_battery_status_pack_chan( - _vehicleSystemId, - _vehicleComponentId, - static_cast(mavlinkChannel()), - &msg, - 1, // battery id - MAV_BATTERY_FUNCTION_ALL, - MAV_BATTERY_TYPE_LIPO, - 2100, // temp cdegC - rgVoltages, - 600, // battery cA - 100, // current consumed mAh - -1, // energy consumed not supported - _battery1PctRemaining, - _battery1TimeRemaining, - _battery1ChargeState, - rgVoltagesExtNone, - 0, // MAV_BATTERY_MODE - 0); // MAV_BATTERY_FAULT - respondWithMavlinkMessage(msg); - mavlink_msg_battery_status_pack_chan( - _vehicleSystemId, - _vehicleComponentId, - static_cast(mavlinkChannel()), - &msg, - 2, // battery id - MAV_BATTERY_FUNCTION_ALL, - MAV_BATTERY_TYPE_LIPO, - INT16_MAX, // temp cdegC - rgVoltagesNone, - 600, // battery cA - 100, // current consumed mAh - -1, // energy consumed not supported - _battery2PctRemaining, - _battery2TimeRemaining, - _battery2ChargeState, - rgVoltagesExtNone, - 0, // MAV_BATTERY_MODE - 0); // MAV_BATTERY_FAULT + (void) mavlink_msg_battery_status_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + static_cast(mavlinkChannel()), + &msg, + 1, // battery id + MAV_BATTERY_FUNCTION_ALL, + MAV_BATTERY_TYPE_LIPO, + 2100, // temp cdegC + rgVoltages, + 600, // battery cA + 100, // current consumed mAh + -1, // energy consumed not supported + _battery1PctRemaining, + _battery1TimeRemaining, + _battery1ChargeState, + rgVoltagesExtNone, + 0, // MAV_BATTERY_MODE + 0 // MAV_BATTERY_FAULT + ); + respondWithMavlinkMessage(msg); + (void) mavlink_msg_battery_status_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + static_cast(mavlinkChannel()), + &msg, + 2, // battery id + MAV_BATTERY_FUNCTION_ALL, + MAV_BATTERY_TYPE_LIPO, + INT16_MAX, // temp cdegC + rgVoltagesNone, + 600, // battery cA + 100, // current consumed mAh + -1, // energy consumed not supported + _battery2PctRemaining, + _battery2TimeRemaining, + _battery2ChargeState, + rgVoltagesExtNone, + 0, // MAV_BATTERY_MODE + 0 // MAV_BATTERY_FAULT + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendVibration(void) -{ - mavlink_message_t msg; - - mavlink_msg_vibration_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - 0, // time_usec - 50.5, // vibration_x, - 10.5, // vibration_y, - 60.0, // vibration_z, - 1, // clipping_0 - 2, // clipping_0 - 3); // clipping_0 - +void MockLink::_sendVibration() +{ + mavlink_message_t msg{}; + (void) mavlink_msg_vibration_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // time_usec + 50.5, // vibration_x, + 10.5, // vibration_y, + 60.0, // vibration_z, + 1, // clipping_0 + 2, // clipping_0 + 3 // clipping_0 + ); respondWithMavlinkMessage(msg); } -void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg) +void MockLink::respondWithMavlinkMessage(const mavlink_message_t &msg) { if (!_commLost) { - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - - int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg); - QByteArray bytes((char *)buffer, cBuffer); + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]{}; + const int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg); + const QByteArray bytes(reinterpret_cast(buffer), cBuffer); emit bytesReceived(this, bytes); } } -/// @brief Called when QGC wants to write bytes to the MAV void MockLink::_writeBytes(const QByteArray &bytes) { // This prevents the responses to mavlink messages from being sent until the _writeBytes returns. emit writeBytesQueuedSignal(bytes); } -void MockLink::_writeBytesQueued(const QByteArray bytes) +void MockLink::_writeBytesQueued(const QByteArray &bytes) { if (_inNSH) { _handleIncomingNSHBytes(bytes.constData(), bytes.length()); - } else { - if (bytes.startsWith(QByteArray("\r\r\r"))) { - _inNSH = true; - _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3); - } + return; + } - _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.length()); + if (bytes.startsWith(QByteArray("\r\r\r"))) { + _inNSH = true; + _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3); } + + _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.length()); } -/// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell -void MockLink::_handleIncomingNSHBytes(const char* bytes, int cBytes) +void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes) { Q_UNUSED(cBytes); // Drop back out of NSH - if (cBytes == 4 && bytes[0] == '\r' && bytes[1] == '\r' && bytes[2] == '\r') { + if ((cBytes == 4) && (bytes[0] == '\r') && (bytes[1] == '\r') && (bytes[2] == '\r')) { _inNSH = false; return; } if (cBytes > 0) { qCDebug(MockLinkLog) << "NSH:" << (const char*)bytes; - #if 0 // MockLink not quite ready to handle this correctly yet if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) { @@ -591,18 +550,14 @@ void MockLink::_handleIncomingNSHBytes(const char* bytes, int cBytes) } } -/// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol -void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) +void MockLink::_handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes) { - QMutexLocker lock{&_mavlinkAuxMutex}; - mavlink_message_t msg; - mavlink_status_t comm; - memset(&comm, 0, sizeof(comm)); - int parsed = 0; + mavlink_message_t msg{}; + mavlink_status_t comm{}; - for (qint64 i=0; ihandleMessage(msg)) { return; } @@ -657,15 +612,15 @@ void MockLink::_handleIncomingMavlinkMsg(const mavlink_message_t &msg) } } -void MockLink::_handleHeartBeat(const mavlink_message_t& msg) +void MockLink::_handleHeartBeat(const mavlink_message_t &msg) { Q_UNUSED(msg); qCDebug(MockLinkLog) << "Heartbeat"; } -void MockLink::_handleParamMapRC(const mavlink_message_t& msg) +void MockLink::_handleParamMapRC(const mavlink_message_t &msg) { - mavlink_param_map_rc_t paramMapRC; + mavlink_param_map_rc_t paramMapRC{}; mavlink_msg_param_map_rc_decode(&msg, ¶mMapRC); const QString paramName(QString::fromLocal8Bit(paramMapRC.param_id, static_cast(strnlen(paramMapRC.param_id, MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN)))); @@ -679,9 +634,9 @@ void MockLink::_handleParamMapRC(const mavlink_message_t& msg) } } -void MockLink::_handleSetMode(const mavlink_message_t& msg) +void MockLink::_handleSetMode(const mavlink_message_t &msg) { - mavlink_set_mode_t request; + mavlink_set_mode_t request{}; mavlink_msg_set_mode_decode(&msg, &request); Q_ASSERT(request.target_system == _vehicleSystemId); @@ -690,28 +645,24 @@ void MockLink::_handleSetMode(const mavlink_message_t& msg) _mavCustomMode = request.custom_mode; } -void MockLink::_handleManualControl(const mavlink_message_t& msg) +void MockLink::_handleManualControl(const mavlink_message_t &msg) { - mavlink_manual_control_t manualControl; + mavlink_manual_control_t manualControl{}; mavlink_msg_manual_control_decode(&msg, &manualControl); qCDebug(MockLinkLog) << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r; } -void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat) +void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString ¶mName, float paramFloat) { - mavlink_param_union_t valueUnion; - Q_ASSERT(_mapParamName2Value.contains(componentId)); Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); - valueUnion.param_float = paramFloat; - - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; - + const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; QVariant paramVariant; - + mavlink_param_union_t valueUnion{}; + valueUnion.param_float = paramFloat; switch (paramType) { case MAV_PARAM_TYPE_REAL32: paramVariant = QVariant::fromValue(valueUnion.param_float); @@ -728,21 +679,17 @@ void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramN case MAV_PARAM_TYPE_UINT16: paramVariant = QVariant::fromValue(valueUnion.param_uint16); break; - case MAV_PARAM_TYPE_INT16: paramVariant = QVariant::fromValue(valueUnion.param_int16); break; - case MAV_PARAM_TYPE_UINT8: paramVariant = QVariant::fromValue(valueUnion.param_uint8); break; - case MAV_PARAM_TYPE_INT8: paramVariant = QVariant::fromValue(valueUnion.param_int8); break; - default: - qCritical() << "Invalid parameter type" << paramType; + qCCritical(MockLinkLog) << "Invalid parameter type" << paramType; paramVariant = QVariant::fromValue(valueUnion.param_int32); break; } @@ -751,23 +698,20 @@ void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramN _mapParamName2Value[componentId][paramName] = paramVariant; } -/// Convert from a parameter variant to the float value from mavlink_param_union_t -float MockLink::_floatUnionForParam(int componentId, const QString& paramName) +float MockLink::_floatUnionForParam(int componentId, const QString ¶mName) { - mavlink_param_union_t valueUnion; - Q_ASSERT(_mapParamName2Value.contains(componentId)); Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; - QVariant paramVar = _mapParamName2Value[componentId][paramName]; + const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; + const QVariant paramVar = _mapParamName2Value[componentId][paramName]; + mavlink_param_union_t valueUnion{}; switch (paramType) { case MAV_PARAM_TYPE_REAL32: valueUnion.param_float = paramVar.toFloat(); break; - case MAV_PARAM_TYPE_UINT32: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toUInt(); @@ -775,7 +719,6 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_uint32 = paramVar.toUInt(); } break; - case MAV_PARAM_TYPE_INT32: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toInt(); @@ -783,7 +726,6 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_int32 = paramVar.toInt(); } break; - case MAV_PARAM_TYPE_UINT16: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toUInt(); @@ -791,7 +733,6 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_uint16 = paramVar.toUInt(); } break; - case MAV_PARAM_TYPE_INT16: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toInt(); @@ -799,7 +740,6 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_int16 = paramVar.toInt(); } break; - case MAV_PARAM_TYPE_UINT8: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toUInt(); @@ -807,7 +747,6 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_uint8 = paramVar.toUInt(); } break; - case MAV_PARAM_TYPE_INT8: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1(); @@ -815,27 +754,25 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1(); } break; - default: if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toInt(); } else { valueUnion.param_int32 = paramVar.toInt(); } - qCritical() << "Invalid parameter type" << paramType; + qCCritical(MockLinkLog) << "Invalid parameter type" << paramType; } return valueUnion.param_float; } -void MockLink::_handleParamRequestList(const mavlink_message_t& msg) +void MockLink::_handleParamRequestList(const mavlink_message_t &msg) { if (_failureMode == MockConfiguration::FailParamNoReponseToRequestList) { return; } - mavlink_param_request_list_t request; - + mavlink_param_request_list_t request{}; mavlink_msg_param_request_list_decode(&msg, &request); Q_ASSERT(request.target_system == _vehicleSystemId); @@ -846,44 +783,45 @@ void MockLink::_handleParamRequestList(const mavlink_message_t& msg) _currentParamRequestListParamIndex = 0; } -/// Sends the next parameter to the vehicle -void MockLink::_paramRequestListWorker(void) +void MockLink::_paramRequestListWorker() { if (_currentParamRequestListComponentIndex == -1) { // Initial request complete return; } - int componentId = _mapParamName2Value.keys()[_currentParamRequestListComponentIndex]; - int cParameters = _mapParamName2Value[componentId].count(); - QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex]; + const int componentId = _mapParamName2Value.keys()[_currentParamRequestListComponentIndex]; + const int cParameters = _mapParamName2Value[componentId].count(); + const QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex]; if ((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest || _failureMode == MockConfiguration::FailMissingParamOnAllRequests) && paramName == _failParam) { qCDebug(MockLinkLog) << "Skipping param send:" << paramName; } else { - char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - mavlink_message_t responseMsg; + char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{}; + mavlink_message_t responseMsg{}; Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName)); - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; + const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName]; Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN); - strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); + (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId]; - mavlink_msg_param_value_pack_chan(_vehicleSystemId, - componentId, // component id - mavlinkChannel(), - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramName), // Parameter value - paramType, // MAV_PARAM_TYPE - cParameters, // Total number of parameters - _currentParamRequestListParamIndex); // Index of this parameter + (void) mavlink_msg_param_value_pack_chan( + _vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramName), // Parameter value + paramType, // MAV_PARAM_TYPE + cParameters, // Total number of parameters + _currentParamRequestListParamIndex // Index of this parameter + ); respondWithMavlinkMessage(responseMsg); } @@ -899,18 +837,18 @@ void MockLink::_paramRequestListWorker(void) } } -void MockLink::_handleParamSet(const mavlink_message_t& msg) +void MockLink::_handleParamSet(const mavlink_message_t &msg) { - mavlink_param_set_t request; + mavlink_param_set_t request{}; mavlink_msg_param_set_decode(&msg, &request); Q_ASSERT(request.target_system == _vehicleSystemId); int componentId = request.target_component; // Param may not be null terminated if exactly fits - char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]; + char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]{}; paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0; - strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN); + (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN); qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type; @@ -924,59 +862,62 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg) // Respond with a param_value to ack mavlink_message_t responseMsg; - mavlink_msg_param_value_pack_chan(_vehicleSystemId, - componentId, // component id - mavlinkChannel(), - &responseMsg, // Outgoing message - paramId, // Parameter name - request.param_value, // Send same value back - request.param_type, // Send same type back - _mapParamName2Value[componentId].count(), // Total number of parameters - _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter + mavlink_msg_param_value_pack_chan( + _vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + request.param_value, // Send same value back + request.param_type, // Send same type back + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter + ); respondWithMavlinkMessage(responseMsg); } -void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) +void MockLink::_handleParamRequestRead(const mavlink_message_t &msg) { - mavlink_message_t responseMsg; - mavlink_param_request_read_t request; + mavlink_message_t responseMsg{}; + mavlink_param_request_read_t request{}; mavlink_msg_param_request_read_decode(&msg, &request); const QString paramName(QString::fromLocal8Bit(request.param_id, static_cast(strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)))); - int componentId = request.target_component; + const int componentId = request.target_component; // special case for magic _HASH_CHECK value - if (request.target_component == MAV_COMP_ID_ALL && paramName == "_HASH_CHECK") { - mavlink_param_union_t valueUnion; + if ((request.target_component == MAV_COMP_ID_ALL) && (paramName == "_HASH_CHECK")) { + mavlink_param_union_t valueUnion{}; valueUnion.type = MAV_PARAM_TYPE_UINT32; valueUnion.param_uint32 = 0; // Special case of magic hash check value - mavlink_msg_param_value_pack_chan(_vehicleSystemId, - componentId, - mavlinkChannel(), - &responseMsg, - request.param_id, - valueUnion.param_float, - MAV_PARAM_TYPE_UINT32, - 0, - -1); + (void) mavlink_msg_param_value_pack_chan( + _vehicleSystemId, + componentId, + mavlinkChannel(), + &responseMsg, + request.param_id, + valueUnion.param_float, + MAV_PARAM_TYPE_UINT32, + 0, + -1 + ); respondWithMavlinkMessage(responseMsg); return; } Q_ASSERT(_mapParamName2Value.contains(componentId)); - char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; + char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]{}; paramId[0] = 0; Q_ASSERT(request.target_system == _vehicleSystemId); if (request.param_index == -1) { // Request is by param name. Param may not be null terminated if exactly fits - strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN); + (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN); } else { // Request is by index - Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count()); QString key = _mapParamName2Value[componentId].keys().at(request.param_index); @@ -987,68 +928,72 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramId)); - if (_failureMode == MockConfiguration::FailMissingParamOnAllRequests && strcmp(paramId, _failParam) == 0) { + if ((_failureMode == MockConfiguration::FailMissingParamOnAllRequests) && (strcmp(paramId, _failParam) == 0)) { qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam; // Fail to send this param no matter what return; } - mavlink_msg_param_value_pack_chan(_vehicleSystemId, - componentId, // component id - mavlinkChannel(), - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramId), // Parameter value - _mapParamName2MavParamType[componentId][paramId], // Parameter type - _mapParamName2Value[componentId].count(), // Total number of parameters - _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter + (void) mavlink_msg_param_value_pack_chan( + _vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramId), // Parameter value + _mapParamName2MavParamType[componentId][paramId], // Parameter type + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter + ); respondWithMavlinkMessage(responseMsg); } void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw) { - uint16_t chanRaw[18]; + uint16_t chanRaw[18]{}; - for (int i=0; i<18; i++) { + for (int i = 0; i < 18; i++) { chanRaw[i] = UINT16_MAX; } chanRaw[channel] = raw; - mavlink_message_t responseMsg; - mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &responseMsg, // Outgoing message - 0, // time since boot, ignored - 18, // channel count - chanRaw[0], // channel raw value - chanRaw[1], // channel raw value - chanRaw[2], // channel raw value - chanRaw[3], // channel raw value - chanRaw[4], // channel raw value - chanRaw[5], // channel raw value - chanRaw[6], // channel raw value - chanRaw[7], // channel raw value - chanRaw[8], // channel raw value - chanRaw[9], // channel raw value - chanRaw[10], // channel raw value - chanRaw[11], // channel raw value - chanRaw[12], // channel raw value - chanRaw[13], // channel raw value - chanRaw[14], // channel raw value - chanRaw[15], // channel raw value - chanRaw[16], // channel raw value - chanRaw[17], // channel raw value - 0); // rss + mavlink_message_t responseMsg{}; + (void) mavlink_msg_rc_channels_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &responseMsg, // Outgoing message + 0, // time since boot, ignored + 18, // channel count + chanRaw[0], // channel raw value + chanRaw[1], // channel raw value + chanRaw[2], // channel raw value + chanRaw[3], // channel raw value + chanRaw[4], // channel raw value + chanRaw[5], // channel raw value + chanRaw[6], // channel raw value + chanRaw[7], // channel raw value + chanRaw[8], // channel raw value + chanRaw[9], // channel raw value + chanRaw[10], // channel raw value + chanRaw[11], // channel raw value + chanRaw[12], // channel raw value + chanRaw[13], // channel raw value + chanRaw[14], // channel raw value + chanRaw[15], // channel raw value + chanRaw[16], // channel raw value + chanRaw[17], // channel raw value + 0 // rss + ); respondWithMavlinkMessage(responseMsg); } -void MockLink::_handleFTP(const mavlink_message_t& msg) +void MockLink::_handleFTP(const mavlink_message_t &msg) { _mockLinkFTP->mavlinkMessageReceived(msg); } -void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t& request) +void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &request) { uint8_t commandResult = MAV_RESULT_UNSUPPORTED; @@ -1066,48 +1011,52 @@ void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t& reques break; } - mavlink_message_t commandAck; - - mavlink_msg_command_ack_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &commandAck, - request.command, - MAV_RESULT_IN_PROGRESS, - 1, // progress - 0, // result_param2 - 0, // target_system - 0); // target_component + mavlink_message_t commandAck{}; + (void) mavlink_msg_command_ack_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + MAV_RESULT_IN_PROGRESS, + 1, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component + ); respondWithMavlinkMessage(commandAck); if (request.command != MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK) { - mavlink_msg_command_ack_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &commandAck, - request.command, - commandResult, - 0, // progress - 0, // result_param2 - 0, // target_system - 0); // target_component + (void) mavlink_msg_command_ack_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + commandResult, + 0, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component + ); respondWithMavlinkMessage(commandAck); } } -void MockLink::_handleCommandLong(const mavlink_message_t& msg) +void MockLink::_handleCommandLong(const mavlink_message_t &msg) { static bool firstCmdUser3 = true; static bool firstCmdUser4 = true; bool noAck = false; - mavlink_command_long_t request; - uint8_t commandResult = MAV_RESULT_UNSUPPORTED; + mavlink_command_long_t request; mavlink_msg_command_long_decode(&msg, &request); _receivedMavCommandCountMap[static_cast(request.command)]++; + uint8_t commandResult = MAV_RESULT_UNSUPPORTED; + switch (request.command) { case MAV_CMD_COMPONENT_ARM_DISARM: if (request.param1 == 0.0f) { @@ -1188,43 +1137,46 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) return; } - mavlink_message_t commandAck; - mavlink_msg_command_ack_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &commandAck, - request.command, - commandResult, - 0, // progress - 0, // result_param2 - 0, // target_system - 0); // target_component + mavlink_message_t commandAck{}; + (void) mavlink_msg_command_ack_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + commandResult, + 0, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component + ); respondWithMavlinkMessage(commandAck); } void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult) { - mavlink_message_t commandAck; - mavlink_msg_command_ack_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &commandAck, - command, - ackResult, - 0, // progress - 0, // result_param2 - 0, // target_system - 0); // target_component + mavlink_message_t commandAck{}; + (void) mavlink_msg_command_ack_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + command, + ackResult, + 0, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component + ); respondWithMavlinkMessage(commandAck); } -void MockLink::_respondWithAutopilotVersion(void) +void MockLink::_respondWithAutopilotVersion() { - mavlink_message_t msg; - - uint8_t customVersion[8] = { }; + uint8_t customVersion[8]{}; uint32_t flightVersion = 0; -#if !defined(NO_ARDUPILOT_DIALECT) + +#ifndef NO_ARDUPILOT_DIALECT if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { if (_vehicleType == MAV_TYPE_FIXED_WING) { flightVersion |= 9 << (8*2); @@ -1244,124 +1196,122 @@ void MockLink::_respondWithAutopilotVersion(void) flightVersion |= 4 << (8*2); flightVersion |= 1 << (8*1); flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); -#if !defined(NO_ARDUPILOT_DIALECT) +#ifndef NO_ARDUPILOT_DIALECT } #endif - uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT | - (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0); - - mavlink_msg_autopilot_version_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - capabilities, - flightVersion, // flight_sw_version, - 0, // middleware_sw_version, - 0, // os_sw_version, - 0, // board_version, - (uint8_t *)&customVersion, // flight_custom_version, - (uint8_t *)&customVersion, // middleware_custom_version, - (uint8_t *)&customVersion, // os_custom_version, - _boardVendorId, - _boardProductId, - 0, // uid - 0); // uid2 + const uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0); + + mavlink_message_t msg{}; + (void) mavlink_msg_autopilot_version_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + capabilities, + flightVersion, // flight_sw_version, + 0, // middleware_sw_version, + 0, // os_sw_version, + 0, // board_version, + reinterpret_cast(&customVersion), // flight_custom_version, + reinterpret_cast(&customVersion), // middleware_custom_version, + reinterpret_cast(&customVersion), // os_custom_version, + _boardVendorId, + _boardProductId, + 0, // uid + 0 + ); // uid2 respondWithMavlinkMessage(msg); } -void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) -{ - _missionItemHandler.setFailureMode(failureMode, failureAckResult); -} - -void MockLink::_sendHomePosition(void) -{ - mavlink_message_t msg; - - float bogus[4]; - bogus[0] = 0.0f; - bogus[1] = 0.0f; - bogus[2] = 0.0f; - bogus[3] = 0.0f; - - mavlink_msg_home_position_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_defaultVehicleHomeAltitude * 1000), - 0.0f, 0.0f, 0.0f, - &bogus[0], - 0.0f, 0.0f, 0.0f, - 0); +void MockLink::_sendHomePosition() +{ + float bogus[4]{}; + + mavlink_message_t msg{}; + (void) mavlink_msg_home_position_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + static_cast(_vehicleLatitude * 1E7), + static_cast(_vehicleLongitude * 1E7), + static_cast(_defaultVehicleHomeAltitude * 1000), + 0.0f, 0.0f, 0.0f, + &bogus[0], + 0.0f, 0.0f, 0.0f, + 0 + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendGpsRawInt(void) +void MockLink::_sendGpsRawInt() { static uint64_t timeTick = 0; - mavlink_message_t msg; - - mavlink_msg_gps_raw_int_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - timeTick++, // time since boot - GPS_FIX_TYPE_3D_FIX, - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_vehicleAltitudeAMSL * 1000), - 3 * 100, // hdop - 3 * 100, // vdop - UINT16_MAX, // velocity not known - UINT16_MAX, // course over ground not known - 8, // satellites visible - //-- Extension - 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up). - 0, // Position uncertainty in meters * 1000 (positive for up). - 0, // Altitude uncertainty in meters * 1000 (positive for up). - 0, // Speed uncertainty in meters * 1000 (positive for up). - 0, // Heading / track uncertainty in degrees * 1e5. - 65535); // Yaw not provided + + mavlink_message_t msg{}; + (void) mavlink_msg_gps_raw_int_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + timeTick++, // time since boot + GPS_FIX_TYPE_3D_FIX, + static_cast(_vehicleLatitude * 1E7), + static_cast(_vehicleLongitude * 1E7), + static_cast(_vehicleAltitudeAMSL * 1000), + 3 * 100, // hdop + 3 * 100, // vdop + UINT16_MAX, // velocity not known + UINT16_MAX, // course over ground not known + 8, // satellites visible + //-- Extension + 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up). + 0, // Position uncertainty in meters * 1000 (positive for up). + 0, // Altitude uncertainty in meters * 1000 (positive for up). + 0, // Speed uncertainty in meters * 1000 (positive for up). + 0, // Heading / track uncertainty in degrees * 1e5. + 65535 // Yaw not provided + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendGlobalPositionInt(void) +void MockLink::_sendGlobalPositionInt() { static uint64_t timeTick = 0; - mavlink_message_t msg; - - mavlink_msg_global_position_int_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - timeTick++, // time since boot - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_vehicleAltitudeAMSL * 1000), - (int32_t)((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000), - 0, 0, 0, // no speed sent - UINT16_MAX); // no heading sent + + mavlink_message_t msg{0}; + (void) mavlink_msg_global_position_int_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + timeTick++, // time since boot + static_cast(_vehicleLatitude * 1E7), + static_cast(_vehicleLongitude * 1E7), + static_cast(_vehicleAltitudeAMSL * 1000), + static_cast((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000), + 0, 0, 0, // no speed sent + UINT16_MAX // no heading sent + ); respondWithMavlinkMessage(msg); } -void MockLink::_sendExtendedSysState(void) +void MockLink::_sendExtendedSysState() { - mavlink_message_t msg; - - mavlink_msg_extended_sys_state_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - MAV_VTOL_STATE_UNDEFINED, - _vehicleAltitudeAMSL > _defaultVehicleHomeAltitude ? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND); + mavlink_message_t msg{}; + (void) mavlink_msg_extended_sys_state_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + MAV_VTOL_STATE_UNDEFINED, + (_vehicleAltitudeAMSL > _defaultVehicleHomeAltitude) ? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND + ); respondWithMavlinkMessage(msg); } void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) { - mavlink_message_t msg; int cChunks = 4; int num = 0; @@ -1369,13 +1319,15 @@ void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) if (missingChunks && (i & 1)) { continue; } + int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - memset(msgBuf, 0, sizeof(msgBuf)); + char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]{}; + if (i == cChunks - 1) { // Last chunk is partial cBuf /= 2; } + for (int j=0; j 9) { @@ -1384,51 +1336,55 @@ void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) } msgBuf[cBuf-1] = 'A' + i; - mavlink_msg_statustext_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - MAV_SEVERITY_INFO, - msgBuf, - chunkId, - i); // chunk sequence number + mavlink_message_t msg{}; + (void) mavlink_msg_statustext_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + MAV_SEVERITY_INFO, + msgBuf, + chunkId, + i // chunk sequence number + ); respondWithMavlinkMessage(msg); } } -void MockLink::_sendStatusTextMessages(void) +void MockLink::_sendStatusTextMessages() { struct StatusMessage { - MAV_SEVERITY severity; - const char* msg; + MAV_SEVERITY severity; + const char *msg; }; - static const struct StatusMessage rgMessages[] = { - { MAV_SEVERITY_INFO, "#Testing audio output" }, - { MAV_SEVERITY_EMERGENCY, "Status text emergency" }, - { MAV_SEVERITY_ALERT, "Status text alert" }, - { MAV_SEVERITY_CRITICAL, "Status text critical" }, - { MAV_SEVERITY_ERROR, "Status text error" }, - { MAV_SEVERITY_WARNING, "Status text warning" }, - { MAV_SEVERITY_NOTICE, "Status text notice" }, - { MAV_SEVERITY_INFO, "Status text info" }, - { MAV_SEVERITY_DEBUG, "Status text debug" }, -}; - - mavlink_message_t msg; - - for (size_t i=0; iseverity, - status->msg, - 0, // Not a chunked sequence - 0); // Not a chunked sequence + (void) mavlink_msg_statustext_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + status->severity, + status->msg, + 0, // Not a chunked sequence + 0 // Not a chunked sequence + ); respondWithMavlinkMessage(msg); } @@ -1438,62 +1394,6 @@ void MockLink::_sendStatusTextMessages(void) _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire } -MockConfiguration::MockConfiguration(const QString& name) - : LinkConfiguration(name) -{ - -} - -MockConfiguration::MockConfiguration(const MockConfiguration* source) - : LinkConfiguration(source) -{ - _firmwareType = source->_firmwareType; - _vehicleType = source->_vehicleType; - _sendStatusText = source->_sendStatusText; - _incrementVehicleId = source->_incrementVehicleId; - _failureMode = source->_failureMode; -} - -void MockConfiguration::copyFrom(const LinkConfiguration *source) -{ - LinkConfiguration::copyFrom(source); - const MockConfiguration* usource = qobject_cast(source); - - if (!usource) { - qCWarning(MockLinkLog) << "dynamic_cast failed" << source << usource; - return; - } - - _firmwareType = usource->_firmwareType; - _vehicleType = usource->_vehicleType; - _sendStatusText = usource->_sendStatusText; - _incrementVehicleId = usource->_incrementVehicleId; - _failureMode = usource->_failureMode; -} - -void MockConfiguration::saveSettings(QSettings& settings, const QString& root) -{ - settings.beginGroup(root); - settings.setValue(_firmwareTypeKey, (int)_firmwareType); - settings.setValue(_vehicleTypeKey, (int)_vehicleType); - settings.setValue(_sendStatusTextKey, _sendStatusText); - settings.setValue(_incrementVehicleIdKey, _incrementVehicleId); - settings.setValue(_failureModeKey, (int)_failureMode); - settings.sync(); - settings.endGroup(); -} - -void MockConfiguration::loadSettings(QSettings& settings, const QString& root) -{ - settings.beginGroup(root); - _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt(); - _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt(); - _sendStatusText = settings.value(_sendStatusTextKey, false).toBool(); - _incrementVehicleId = settings.value(_incrementVehicleIdKey, true).toBool(); - _failureMode = (FailureMode_t)settings.value(_failureModeKey, (int)FailNone).toInt(); - settings.endGroup(); -} - MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) { mockConfig->setDynamic(true); @@ -1506,9 +1406,9 @@ MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) } } -MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - MockConfiguration* mockConfig = new MockConfiguration(configName); + MockConfiguration *const mockConfig = new MockConfiguration(configName); mockConfig->setFirmwareType(firmwareType); mockConfig->setVehicleType(vehicleType); @@ -1518,88 +1418,88 @@ MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmw return _startMockLink(mockConfig); } -MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("PX4 MultiRotor MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } -MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("Generic MockLink"), MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } -MockLink* MockLink::startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("No Initial Connect MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("No Initial Connect MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode); } -MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduCopter MockLink"),MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } -MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduPlane MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduPlane MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode); } -MockLink* MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduSub MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduSub MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode); } -MockLink* MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduRover MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduRover MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode); } -void MockLink::_sendRCChannels(void) +void MockLink::_sendRCChannels() { - mavlink_message_t msg; - - mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - 0, // time_boot_ms - 16, // chancount - 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 1-8 - 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 9-16 - UINT16_MAX, UINT16_MAX, - 0); // rssi - + mavlink_message_t msg{}; + (void) mavlink_msg_rc_channels_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // time_boot_ms + 16, // chancount + 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 1-8 + 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 9-16 + UINT16_MAX, UINT16_MAX, + 0 // rssi + ); respondWithMavlinkMessage(msg); } void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request) { - const char* pCalMessage; - static const char* gyroCalResponse = "[cal] calibration started: 2 gyro"; - static const char* magCalResponse = "[cal] calibration started: 2 mag"; - static const char* accelCalResponse = "[cal] calibration started: 2 accel"; + static constexpr const char *gyroCalResponse = "[cal] calibration started: 2 gyro"; + static constexpr const char *magCalResponse = "[cal] calibration started: 2 mag"; + static constexpr const char *accelCalResponse = "[cal] calibration started: 2 accel"; + const char *pCalMessage; if (request.param1 == 1) { - // Gyro cal pCalMessage = gyroCalResponse; } else if (request.param2 == 1) { - // Mag cal pCalMessage = magCalResponse; } else if (request.param5 == 1) { - // Accel cal pCalMessage = accelCalResponse; } else { return; } - mavlink_message_t msg; - mavlink_msg_statustext_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &msg, - MAV_SEVERITY_INFO, - pCalMessage, - 0, 0); // Not chunked + mavlink_message_t msg{}; + (void) mavlink_msg_statustext_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + MAV_SEVERITY_INFO, + pCalMessage, + 0, + 0 // Not chunked + ); respondWithMavlinkMessage(msg); } @@ -1611,8 +1511,7 @@ void MockLink::_handleTakeoff(const mavlink_command_long_t& request) void MockLink::_handleLogRequestList(const mavlink_message_t& msg) { - mavlink_log_request_list_t request; - + mavlink_log_request_list_t request{}; mavlink_msg_log_request_list_decode(&msg, &request); if (request.start != 0 && request.end != 0xffff) { @@ -1620,23 +1519,24 @@ void MockLink::_handleLogRequestList(const mavlink_message_t& msg) return; } - mavlink_message_t responseMsg; - mavlink_msg_log_entry_pack_chan(_vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &responseMsg, - _logDownloadLogId, // log id - 1, // num_logs - 1, // last_log_num - 0, // time_utc - _logDownloadFileSize); // size + mavlink_message_t responseMsg{}; + mavlink_msg_log_entry_pack_chan( + _vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &responseMsg, + _logDownloadLogId, // log id + 1, // num_logs + 1, // last_log_num + 0, // time_utc + _logDownloadFileSize // size + ); respondWithMavlinkMessage(responseMsg); } QString MockLink::_createRandomFile(uint32_t byteCount) { QTemporaryFile tempFile; - tempFile.setAutoRemove(false); if (tempFile.open()) { for (uint32_t bytesWritten=0; bytesWritten -#include #include +#include #include #include +#include Q_DECLARE_LOGGING_CATEGORY(MockLinkLog) Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog) -class LinkManager; - -class MockConfiguration : public LinkConfiguration -{ - Q_OBJECT - -public: - MockConfiguration(const QString& name); - MockConfiguration(const 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; } - uint16_t boardVendorId (void) { return _boardVendorId; } - uint16_t boardProductId (void) { return _boardProductId; } - MAV_TYPE vehicleType (void) { return _vehicleType; } - bool sendStatusText (void) const { return _sendStatusText; } - - void setFirmwareType (MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); } - void setBoardVendorProduct(uint16_t vendorId, uint16_t productId) { _boardVendorId = vendorId; _boardProductId = productId; } - 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 - FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure - FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent - FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure - FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent - } FailureMode_t; - FailureMode_t failureMode(void) { return _failureMode; } - void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } - - // Overrides from LinkConfiguration - LinkType type (void) const override { return LinkConfiguration::TypeMock; } - void copyFrom (const 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; - uint16_t _boardVendorId = 0; - uint16_t _boardProductId = 0; - - static constexpr const char* _firmwareTypeKey = "FirmwareType"; - static constexpr const char* _vehicleTypeKey = "VehicleType"; - static constexpr const char* _sendStatusTextKey = "SendStatusText"; - static constexpr const char* _incrementVehicleIdKey = "IncrementVehicleId"; - static constexpr const char* _failureModeKey = "FailureMode"; -}; - class MockLink : public LinkInterface { Q_OBJECT public: - MockLink(SharedLinkConfigurationPtr& config); + explicit MockLink(SharedLinkConfigurationPtr &config, QObject *parent = nullptr); 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; } + bool isConnected() const override { return _connected; } + void disconnect() override; + + int vehicleId() const { return _vehicleSystemId; } + MAV_AUTOPILOT getFirmwareType() const { 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 @@ -124,13 +51,9 @@ class MockLink : public LinkInterface 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; } + void respondWithMavlinkMessage(const mavlink_message_t &msg); - // Overrides from LinkInterface - bool isConnected(void) const override { return _connected; } - void disconnect (void) override; + MockLinkFTP *mockLinkFTP() const { return _mockLinkFTP; } struct ADSBVehicle { QGeoCoordinate coordinate; @@ -138,208 +61,217 @@ class MockLink : public LinkInterface double altitude; // Store unique altitude for each vehicle }; std::vector _adsbVehicles; // Store data for multiple ADS-B vehicles - + /// 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); + void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) const { _missionItemHandler->setFailureMode(failureMode, failureAckResult); } /// Called to send a MISSION_ACK message while the MissionManager is in idle state - void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } + void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) const { _missionItemHandler->sendUnexpectedMissionAck(ackType); } /// Called to send a MISSION_ITEM message while the MissionManager is in idle state - void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } + void sendUnexpectedMissionItem() const { _missionItemHandler->sendUnexpectedMissionItem(); } /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state - void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } + void sendUnexpectedMissionRequest() const { _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(); } + void resetMissionItemHandler() const { _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); + QString logDownloadFile() const { return _logDownloadFilename; } + + Q_INVOKABLE void setCommLost(bool commLost) { _commLost = commLost; } + Q_INVOKABLE void simulateConnectionRemoved(); + + 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 Vehicle::sendMavCommandWithHandler - 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_USER_5 + 1); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast(MAV_CMD_USER_5 + 2); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast(MAV_CMD_USER_5 + 3); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast(MAV_CMD_USER_5 + 4); - - void clearReceivedMavCommandCounts(void) { _receivedMavCommandCountMap.clear(); } - int receivedMavCommandCount(MAV_CMD command) { return _receivedMavCommandCountMap[command]; } - - typedef enum { + 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_USER_5 + 1); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast(MAV_CMD_USER_5 + 2); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast(MAV_CMD_USER_5 + 3); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast(MAV_CMD_USER_5 + 4); + + void clearReceivedMavCommandCounts() { _receivedMavCommandCountMap.clear(); } + int receivedMavCommandCount(MAV_CMD command) const { return _receivedMavCommandCountMap[command]; } + + enum RequestMessageFailureMode_t { FailRequestMessageNone, FailRequestMessageCommandAcceptedMsgNotSent, FailRequestMessageCommandUnsupported, FailRequestMessageCommandNoResponse, - } RequestMessageFailureMode_t; + }; void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; } signals: - void writeBytesQueuedSignal (const QByteArray bytes); + void writeBytesQueuedSignal(const QByteArray bytes); void highLatencyTransmissionEnabledChanged (bool highLatencyTransmissionEnabled); private slots: - // LinkInterface overrides + /// @brief Called when QGC wants to write bytes to the MAV void _writeBytes(const QByteArray &bytes) final; - void _writeBytesQueued (const QByteArray bytes); - void _run1HzTasks (void); - void _run10HzTasks (void); - void _run500HzTasks (void); - void _sendStatusTextMessages(void); + void _writeBytesQueued(const QByteArray &bytes); + void _run1HzTasks(); + void _run10HzTasks(); + void _run500HzTasks(); + void _sendStatusTextMessages(); private: // LinkInterface overrides - bool _connect (void) override; - bool _allocateMavlinkChannel () override; - void _freeMavlinkChannel () override; - uint8_t mavlinkAuxChannel (void) const; - bool mavlinkAuxChannelIsSet (void) const; + bool _connect() final; + bool _allocateMavlinkChannel() final; + void _freeMavlinkChannel() final; + + uint8_t mavlinkAuxChannel() const { return _mavlinkAuxChannel; } + bool mavlinkAuxChannelIsSet() const; // 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 _handleIncomingMavlinkMsg (const mavlink_message_t& msg); - 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 _handleInProgressCommandLong (const mavlink_command_long_t& request); - void _handleManualControl (const mavlink_message_t& msg); - void _handlePreFlightCalibration (const mavlink_command_long_t& request); - void _handleTakeoff (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 _sendGlobalPositionInt (void); - void _sendExtendedSysState (void); - void _sendVibration (void); - void _sendSysStatus (void); - void _sendBatteryStatus (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 (int vehicleIndex); - void _sendGeneralMetaData (void); - void _sendRemoteIDArmStatus (void); - - static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); - static MockLink* _startMockLink(MockConfiguration* mockConfig); + void run() final; + + void _sendHeartBeat(); + void _sendHighLatency2(); + /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell + void _handleIncomingNSHBytes(const char *bytes, int cBytes); + /// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol + void _handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes); + void _handleIncomingMavlinkMsg(const mavlink_message_t &msg); + void _loadParams(); + 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 _handleInProgressCommandLong(const mavlink_command_long_t &request); + void _handleManualControl(const mavlink_message_t &msg); + void _handlePreFlightCalibration(const mavlink_command_long_t &request); + void _handleTakeoff(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); + /// Convert from a parameter variant to the float value from mavlink_param_union_t + float _floatUnionForParam(int componentId, const QString ¶mName); + void _setParamFloatUnionIntoMap(int componentId, const QString ¶mName, float paramFloat); + void _sendHomePosition(); + void _sendGpsRawInt(); + void _sendGlobalPositionInt(); + void _sendExtendedSysState(); + void _sendVibration(); + void _sendSysStatus(); + void _sendBatteryStatus(); + void _sendChunkedStatusText(uint16_t chunkId, bool missingChunks); + void _respondWithAutopilotVersion(); + void _sendRCChannels(); + /// Sends the next parameter to the vehicle + void _paramRequestListWorker(); + void _logDownloadWorker(); + void _sendADSBVehicles(); + void _moveADSBVehicle(int vehicleIndex); + void _sendGeneralMetaData(); + void _sendRemoteIDArmStatus(); + + static MockLink *_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); + static MockLink *_startMockLink(MockConfiguration *mockConfig); /// Creates a file with random contents of the specified size. /// @return Fully qualified path to created file static QString _createRandomFile(uint32_t byteCount); - uint8_t _mavlinkAuxChannel = std::numeric_limits::max(); - QMutex _mavlinkAuxMutex; + MockLinkMissionItemHandler *_missionItemHandler = nullptr; - MockLinkMissionItemHandler _missionItemHandler; + uint8_t _mavlinkAuxChannel = std::numeric_limits::max(); + QMutex _mavlinkAuxMutex; - QString _name; - bool _connected; + QString _name = QString("MockLink"); + bool _connected = false; - uint8_t _vehicleSystemId; - uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; + uint8_t _vehicleSystemId = 0; + uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; - bool _inNSH; - bool _mavlinkStarted; + bool _inNSH = false; + bool _mavlinkStarted = false; - uint8_t _mavBaseMode; - uint32_t _mavCustomMode; - uint8_t _mavState; + uint8_t _mavBaseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + uint32_t _mavCustomMode = PX4CustomMode::MANUAL; + uint8_t _mavState = MAV_STATE_STANDBY; - 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; + QElapsedTimer _runningTime; + static constexpr 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 _vehicleAltitudeAMSL; - bool _commLost = false; - bool _highLatencyTransmissionEnabled = true; + MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; + MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; + double _vehicleLatitude = 0.0; + double _vehicleLongitude = 0.0; + double _vehicleAltitudeAMSL = _defaultVehicleHomeAltitude; + bool _commLost = false; + bool _highLatencyTransmissionEnabled = true; // These are just set for reporting the fields in _respondWithAutopilotVersion() // and ensuring that the Vehicle reports the fields in Vehicle::firmwareBoardVendorId etc. // They do not control any mock simulation (and it is up to the Custom build to do that). - uint16_t _boardVendorId = 0; - uint16_t _boardProductId = 0; + uint16_t _boardVendorId = 0; + uint16_t _boardProductId = 0; - MockLinkFTP* _mockLinkFTP = nullptr; + MockLinkFTP *_mockLinkFTP = nullptr; - bool _sendStatusText; - bool _apmSendHomePositionOnEmptyList; - MockConfiguration::FailureMode_t _failureMode; + bool _sendStatusText = false; + bool _apmSendHomePositionOnEmptyList = false; + MockConfiguration::FailureMode_t _failureMode = MockConfiguration::FailNone; - int _sendHomePositionDelayCount; - int _sendGPSPositionDelayCount; + int _sendHomePositionDelayCount = 10; ///< No home position for 4 seconds + int _sendGPSPositionDelayCount = 100; ///< No gps lock for 5 seconds - 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 + int _currentParamRequestListComponentIndex = -1; ///< Current component index for param request list workflow, -1 for no request in progress + int _currentParamRequestListParamIndex = -1; ///< 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 + static constexpr uint16_t _logDownloadLogId = 0; ///< Id of siumulated log file + static constexpr 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 + QString _logDownloadFilename; ///< Filename for log download which is in progress + uint32_t _logDownloadCurrentOffset = 0; ///< Current offset we are sending from + uint32_t _logDownloadBytesRemaining = 0; ///< Number of bytes still to send, 0 = send inactive - QList _adsbVehicleCoordinates; // List for multiple vehicles - double _adsbAngles[5]; // Array for angles of each vehicle - static constexpr int _numberOfVehicles = 5; // Number of ADS-B vehicles + QList _adsbVehicleCoordinates; ///< List for multiple vehicles + static constexpr int _numberOfVehicles = 5; ///< Number of ADS-B vehicles + double _adsbAngles[_numberOfVehicles]{}; ///< Array for angles of each vehicle RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone; - QMap _receivedMavCommandCountMap; - QMap> _mapParamName2Value; - QMap> _mapParamName2MavParamType; + QMap _receivedMavCommandCountMap; + QMap> _mapParamName2Value; + QMap> _mapParamName2MavParamType; + + static int _nextVehicleSystemId; + + // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle + // testing of a gazebo vehicle and a mocklink vehicle + static constexpr double _defaultVehicleLatitude = 47.397; + static constexpr double _defaultVehicleLongitude = 8.5455; + static constexpr double _defaultVehicleHomeAltitude = 488.056; - static double _defaultVehicleLatitude; - static double _defaultVehicleLongitude; - static double _defaultVehicleHomeAltitude; - static int _nextVehicleSystemId; - static constexpr const char* _failParam = "COM_FLTMODE6"; + static constexpr const char *_failParam = "COM_FLTMODE6"; };