diff --git a/ChangeLog.md b/ChangeLog.md index d49e683..9d2611a 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -7,6 +7,7 @@ Note: This file only contains high level features or important fixes. ### 3.6.0 - Daily Build * More performant flight path display algorithm. Mobile builds no longer show limited path length. +* ArduCopter/Rover: Add support for Follow Me * ArduPilot: Add Motor Test vehicle setup page * Compass Instrument: Add indicators for Home, COG and Next Waypoint headings. * Log Replay: Support changing speed of playback diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 64a4962..2f2f7a8 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -1086,3 +1086,47 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t &channels); } +void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) +{ + if (!vehicle->homePosition().isValid()) { + static bool sentOnce = false; + if (!sentOnce) { + sentOnce = true; + qgcApp()->showMessage(tr("Follow failed: Home position not set.")); + } + return; + } + + if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL))) { + static bool sentOnce = false; + if (!sentOnce) { + sentOnce = true; + qWarning() << "APMFirmwarePlugin::_sendGCSMotionReport estimateCapabilities" << estimationCapabilities; + qgcApp()->showMessage(tr("Follow failed: Ground station cannot provide required position information.")); + } + return; + } + + MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + + mavlink_global_position_int_t globalPositionInt; + memset(&globalPositionInt, 0, sizeof(globalPositionInt)); + + globalPositionInt.time_boot_ms = static_cast(qgcApp()->msecsSinceBoot()); + globalPositionInt.lat = motionReport.lat_int; + globalPositionInt.lon = motionReport.lon_int; + globalPositionInt.alt = static_cast(motionReport.altMetersAMSL * 1000); // mm + globalPositionInt.relative_alt = static_cast((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm + globalPositionInt.vx = static_cast(motionReport.vxMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vyMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vzMetersPerSec * 100); // cm/sec + globalPositionInt.hdg = UINT16_MAX; + + mavlink_message_t message; + mavlink_msg_global_position_int_encode_chan(static_cast(mavlinkProtocol->getSystemId()), + static_cast(mavlinkProtocol->getComponentId()), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &globalPositionInt); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index fef3418..17aee29 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -17,6 +17,7 @@ #include "FirmwarePlugin.h" #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" +#include "FollowMe.h" #include @@ -109,7 +110,9 @@ public: protected: /// All access to singleton is through stack specific implementation APMFirmwarePlugin(void); - void setSupportedModes(QList supportedModes); + + void setSupportedModes (QList supportedModes); + void _sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities); bool _coaxialMotors; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 10e5abb..378bae1 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -43,7 +43,7 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : { GUIDED_NOGPS, "Guided No GPS"}, { SMART_RTL, "Smart RTL"}, { FLOWHOLD, "Flow Hold" }, - { FOLLOW, "Follow Vehicle" }, + { FOLLOW, "Follow" }, { ZIGZAG, "ZigZag" }, }); } @@ -136,3 +136,7 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* return true; } +void ArduCopterFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) +{ + _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities); +} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index a675e74..08be16f 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -64,12 +64,14 @@ public: bool multiRotorCoaxialMotors (Vehicle* vehicle) final; bool multiRotorXConfig (Vehicle* vehicle) final; QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); } - QString pauseFlightMode (void) const override { return QString("Brake"); } - QString landFlightMode (void) const override { return QString("Land"); } - QString takeControlFlightMode (void) const override { return QString("Loiter"); } + QString pauseFlightMode (void) const override { return QStringLiteral("Brake"); } + QString landFlightMode (void) const override { return QStringLiteral("Land"); } + QString takeControlFlightMode (void) const override { return QStringLiteral("Loiter"); } + QString followFlightMode (void) const override { return QStringLiteral("Follow"); } bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } bool supportsSmartRTL (void) const override { return true; } + void sendGCSMotionReport (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index ddf64d7..3383987 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -77,3 +77,8 @@ bool ArduRoverFirmwarePlugin::supportsNegativeThrust(Vehicle* /*vehicle*/) { return true; } + +void ArduRoverFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) +{ + _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities); +} diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index ad17a03..dea59ff 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -45,13 +45,15 @@ public: ArduRoverFirmwarePlugin(void); // Overrides from FirmwarePlugin - QString pauseFlightMode (void) const override { return QString("Hold"); } + QString pauseFlightMode (void) const override { return QStringLiteral("Hold"); } + QString followFlightMode (void) const override { return QStringLiteral("Follow"); } void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } bool supportsNegativeThrust (Vehicle *) final; bool supportsSmartRTL (void) const override { return true; } QString offlineEditingParamFile (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Rover.OfflineEditing.params"); } + void sendGCSMotionReport (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 0b48c76..6638f8b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -909,3 +909,28 @@ QString FirmwarePlugin::gotoFlightMode(void) const { return QString(); } + +void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) +{ + MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + + mavlink_follow_target_t follow_target = {}; + + follow_target.timestamp = qgcApp()->msecsSinceBoot(); + follow_target.est_capabilities = estimationCapabilities; + follow_target.position_cov[0] = static_cast(motionReport.pos_std_dev[0]); + follow_target.position_cov[2] = static_cast(motionReport.pos_std_dev[2]); + follow_target.alt = static_cast(motionReport.altMetersAMSL); + follow_target.lat = motionReport.lat_int; + follow_target.lon = motionReport.lon_int; + follow_target.vel[0] = static_cast(motionReport.vxMetersPerSec); + follow_target.vel[1] = static_cast(motionReport.vyMetersPerSec); + + mavlink_message_t message; + mavlink_msg_follow_target_encode_chan(static_cast(mavlinkProtocol->getSystemId()), + static_cast(mavlinkProtocol->getComponentId()), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &follow_target); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 51739d4..a6627e6 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -19,6 +19,7 @@ #include "AutoPilotPlugin.h" #include "GeoFenceManager.h" #include "RallyPointManager.h" +#include "FollowMe.h" #include #include @@ -122,6 +123,9 @@ public: /// Returns the flight mode which the vehicle will be in if it is performing a goto location virtual QString gotoFlightMode(void) const; + /// Returns the flight mode which the vehicle will be for follow me + virtual QString followFlightMode(void) const { return QString(); }; + /// Set guided flight mode virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode); @@ -320,6 +324,9 @@ public: /// @param metaData - MetaData for fact virtual void adjustMetaData(MAV_TYPE vehicleType, FactMetaData* metaData) {Q_UNUSED(vehicleType); Q_UNUSED(metaData);} + /// Sends the appropriate mavlink message for follow me support + virtual void sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities); + // FIXME: Hack workaround for non pluginize FollowMe support static const QString px4FollowMeFlightMode; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 19cad42..bd6efcb 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -189,8 +189,6 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c qWarning() << "Unknown flight mode" << custom_mode; return tr("Unknown %1:%2").arg(base_mode).arg(custom_mode); } - } else { - qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?"; } return flightMode; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 05a90b6..cf8d72a 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -43,6 +43,7 @@ public: QString landFlightMode (void) const override { return _landingFlightMode; } QString takeControlFlightMode (void) const override { return _manualFlightMode; } QString gotoFlightMode (void) const override { return _holdFlightMode; } + QString followFlightMode (void) const override { return _followMeFlightMode; }; void pauseVehicle (Vehicle* vehicle) override; void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeLand (Vehicle* vehicle) override; diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 5bf9539..5a8c378 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -81,7 +81,6 @@ FlightMap { // When the user pans the map we stop responding to vehicle coordinate updates until the panRecenterTimer fires onUserPannedChanged: { if (userPanned) { - console.log("user panned") userPanned = false _disableVehicleTracking = true panRecenterTimer.restart() diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index 7505989..2eb0f0e 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -20,176 +20,162 @@ #include "AppSettings.h" FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox), estimatation_capabilities(0) + : QGCTool(app, toolbox) { - memset(&_motionReport, 0, sizeof(motionReport_s)); - runTime.start(); _gcsMotionReportTimer.setSingleShot(false); } void FollowMe::setToolbox(QGCToolbox* toolbox) { QGCTool::setToolbox(toolbox); - connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport); - connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged); - _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); - if(_currentMode == MODE_ALWAYS) { - _enable(); - } -} -void FollowMe::followMeHandleManager(const QString&) -{ - //-- If we are to change based on current flight mode - if(_currentMode == MODE_FOLLOWME) { - QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); - for (int i=0; i< vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { - _enable(); - return; - } - } - _disable(); - } + connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport); + connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged); + + _settingsChanged(); } void FollowMe::_settingsChanged() { - uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); - if(_currentMode != mode) { - _currentMode = mode; - switch (mode) { - case MODE_NEVER: - if(_gcsMotionReportTimer.isActive()) { - _disable(); - } - break; - case MODE_ALWAYS: - if(!_gcsMotionReportTimer.isActive()) { - _enable(); - } - break; - case MODE_FOLLOWME: - if(!_gcsMotionReportTimer.isActive()) { - followMeHandleManager(QString()); - } - break; - default: - break; - } + _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt(); + + switch (_currentMode) { + case MODE_NEVER: + disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _disableFollowSend(); + break; + case MODE_ALWAYS: + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _enableFollowSend(); + break; + case MODE_FOLLOWME: + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &FollowMe::_vehicleAdded); + connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved, this, &FollowMe::_vehicleRemoved); + _enableIfVehicleInFollow(); + break; } } -void FollowMe::_enable() +void FollowMe::_enableFollowSend() { - connect(_toolbox->qgcPositionManager(), - &QGCPositionManager::positionInfoUpdated, - this, - &FollowMe::_setGPSLocation); - _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval()); - _gcsMotionReportTimer.start(); + if (!_gcsMotionReportTimer.isActive()) { + _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250)); + _gcsMotionReportTimer.start(); + } } -void FollowMe::_disable() +void FollowMe::_disableFollowSend() { - disconnect(_toolbox->qgcPositionManager(), - &QGCPositionManager::positionInfoUpdated, - this, - &FollowMe::_setGPSLocation); - _gcsMotionReportTimer.stop(); + if (_gcsMotionReportTimer.isActive()) { + _gcsMotionReportTimer.stop(); + } } -void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo) +void FollowMe::_sendGCSMotionReport() { + QGeoPositionInfo geoPositionInfo = _toolbox->qgcPositionManager()->geoPositionInfo(); + QGeoCoordinate gcsCoordinate = geoPositionInfo.coordinate(); + if (!geoPositionInfo.isValid()) { - //-- Invalidate cached coordinates - memset(&_motionReport, 0, sizeof(motionReport_s)); - } else { - // get the current location coordinates + return; + } - QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate(); + GCSMotionReport motionReport = {}; + uint8_t estimatation_capabilities = 0; - _motionReport.lat_int = geoCoordinate.latitude() * 1e7; - _motionReport.lon_int = geoCoordinate.longitude() * 1e7; - _motionReport.alt = geoCoordinate.altitude(); + // get the current location coordinates - estimatation_capabilities |= (1 << POS); + motionReport.lat_int = static_cast(gcsCoordinate.latitude() * 1e7); + motionReport.lon_int = static_cast(gcsCoordinate.longitude() * 1e7); + motionReport.altMetersAMSL = gcsCoordinate.altitude(); + estimatation_capabilities |= (1 << POS); - // get the current eph and epv + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) { + motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction); + } - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) == true) { - _motionReport.pos_std_dev[0] = _motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy); - } + // get the current eph and epv - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy) == true) { - _motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy); - } + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + motionReport.pos_std_dev[0] = motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy); + } - // calculate z velocity if it's available + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy); + } - if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) { - _motionReport.vz = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed); - } + // calculate z velocity if it's available + + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) { + motionReport.vzMetersPerSec = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed); + } - // calculate x,y velocity if it's available + // calculate x,y velocity if it's available - if((geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) && - (geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true)) { + if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true) { + estimatation_capabilities |= (1 << VEL); - estimatation_capabilities |= (1 << VEL); + qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); + qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); - qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction)); - qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed); + motionReport.vxMetersPerSec = cos(direction)*velocity; + motionReport.vyMetersPerSec = sin(direction)*velocity; + } else { + motionReport.vxMetersPerSec = 0; + motionReport.vyMetersPerSec = 0; + } - _motionReport.vx = cos(direction)*velocity; - _motionReport.vy = sin(direction)*velocity; + QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); - } else { - _motionReport.vx = 0.0f; - _motionReport.vy = 0.0f; + for (int i=0; icount(); i++) { + Vehicle* vehicle = vehicles->value(i); + if (_currentMode == MODE_ALWAYS || (_currentMode == MODE_FOLLOWME && _isFollowFlightMode(vehicle, vehicle->flightMode()))) { + vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities); } } } -void FollowMe::_sendGCSMotionReport() +double FollowMe::_degreesToRadian(double deg) +{ + return deg * M_PI / 180.0; +} + +void FollowMe::_vehicleAdded(Vehicle* vehicle) { + connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow); + _enableIfVehicleInFollow(); +} + +void FollowMe::_vehicleRemoved(Vehicle* vehicle) +{ + disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow); + _enableIfVehicleInFollow(); +} - //-- Do we have any real data? - if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) { +void FollowMe::_enableIfVehicleInFollow(void) +{ + if (_currentMode == MODE_ALWAYS) { + // System always enabled return; } - QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles(); - MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol(); - mavlink_follow_target_t follow_target; - memset(&follow_target, 0, sizeof(follow_target)); - - follow_target.timestamp = runTime.nsecsElapsed() * 1e-6; - follow_target.est_capabilities = estimatation_capabilities; - follow_target.position_cov[0] = _motionReport.pos_std_dev[0]; - follow_target.position_cov[2] = _motionReport.pos_std_dev[2]; - follow_target.alt = _motionReport.alt; - follow_target.lat = _motionReport.lat_int; - follow_target.lon = _motionReport.lon_int; - follow_target.vel[0] = _motionReport.vx; - follow_target.vel[1] = _motionReport.vy; - - for (int i=0; i< vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) { - mavlink_message_t message; - mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &message, - &follow_target); - vehicle->sendMessageOnLink(vehicle->priorityLink(), message); + // Any vehicle in follow mode will enable the system + QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); + + for (int i=0; i< vehicles->count(); i++) { + Vehicle* vehicle = vehicles->value(i); + if (_isFollowFlightMode(vehicle, vehicle->flightMode())) { + _enableFollowSend(); + return; } } + + _disableFollowSend(); } -double FollowMe::_degreesToRadian(double deg) +bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode) { - return deg * M_PI / 180.0; + return flightMode.compare(vehicle->followFlightMode()) == 0; } diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h index 4b744a5..e17bcab 100644 --- a/src/FollowMe/FollowMe.h +++ b/src/FollowMe/FollowMe.h @@ -20,6 +20,8 @@ #include "QGCToolbox.h" #include "MAVLinkProtocol.h" +class Vehicle; + Q_DECLARE_LOGGING_CATEGORY(FollowMeLog) class FollowMe : public QGCTool @@ -29,36 +31,18 @@ class FollowMe : public QGCTool public: FollowMe(QGCApplication* app, QGCToolbox* toolbox); - void setToolbox(QGCToolbox* toolbox) override; - -public slots: - void followMeHandleManager (const QString&); - -private slots: - void _setGPSLocation (QGeoPositionInfo geoPositionInfo); - void _sendGCSMotionReport (); - void _settingsChanged (); - -private: - QElapsedTimer runTime; - QTimer _gcsMotionReportTimer; // Timer to emit motion reports - - struct motionReport_s { - uint32_t timestamp; // time since boot - int32_t lat_int; // X Position in WGS84 frame in 1e7 * meters - int32_t lon_int; // Y Position in WGS84 frame in 1e7 * meters - float alt; // Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT - float vx; // X velocity in NED frame in meter / s - float vy; // Y velocity in NED frame in meter / s - float vz; // Z velocity in NED frame in meter / s - float afx; // X acceleration in NED frame in meter / s^2 or N - float afy; // Y acceleration in NED frame in meter / s^2 or N - float afz; // Z acceleration in NED frame in meter / s^2 or N - float pos_std_dev[3]; // -1 for unknown - } _motionReport; + struct GCSMotionReport { + int lat_int; // X Position in WGS84 frame in 1e7 * meters + int lon_int; // Y Position in WGS84 frame in 1e7 * meters + double altMetersAMSL; // Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT + double headingDegrees; // Heading in degrees + double vxMetersPerSec; // X velocity in NED frame in meter / s + double vyMetersPerSec; // Y velocity in NED frame in meter / s + double vzMetersPerSec; // Z velocity in NED frame in meter / s + double pos_std_dev[3]; // -1 for unknown + }; // Mavlink defined motion reporting capabilities - enum { POS = 0, VEL = 1, @@ -66,18 +50,27 @@ private: ATT_RATES = 3 }; - uint8_t estimatation_capabilities; - - void _disable (); - void _enable (); + void setToolbox(QGCToolbox* toolbox) override; - double _degreesToRadian(double deg); +private slots: + void _sendGCSMotionReport (void); + void _settingsChanged (void); + void _vehicleAdded (Vehicle* vehicle); + void _vehicleRemoved (Vehicle* vehicle); + void _enableIfVehicleInFollow (void); +private: enum { MODE_NEVER, MODE_ALWAYS, MODE_FOLLOWME }; - uint32_t _currentMode; + void _disableFollowSend (void); + void _enableFollowSend (void); + double _degreesToRadian (double deg); + bool _isFollowFlightMode (Vehicle* vehicle, const QString& flightMode); + + QTimer _gcsMotionReportTimer; + uint32_t _currentMode; }; diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 4ed76fd..7d5fab0 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/src/PositionManager/PositionManager.cpp @@ -46,12 +46,11 @@ void QGCPositionManager::setToolbox(QGCToolbox *toolbox) } _simulatedSource = new SimulatedPosition(); - // Enable this to get a simulated target on desktop - // if (_defaultSource == nullptr) { - // _defaultSource = _simulatedSource; - // } - +#if 1 setPositionSource(QGCPositionSource::InternalGPS); +#else + setPositionSource(QGCPositionManager::Simulated); +#endif } void QGCPositionManager::setNmeaSourceDevice(QIODevice* device) @@ -77,6 +76,8 @@ void QGCPositionManager::setNmeaSourceDevice(QIODevice* device) void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update) { + _geoPositionInfo = update; + QGeoCoordinate newGCSPosition = QGeoCoordinate(); qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction); diff --git a/src/PositionManager/PositionManager.h b/src/PositionManager/PositionManager.h index 30c39f3..0f2d3a7 100644 --- a/src/PositionManager/PositionManager.h +++ b/src/PositionManager/PositionManager.h @@ -35,17 +35,16 @@ public: NmeaGPS }; - QGeoCoordinate gcsPosition(void) { return _gcsPosition; } + QGeoCoordinate gcsPosition (void) { return _gcsPosition; } + qreal gcsHeading (void) { return _gcsHeading; } + QGeoPositionInfo geoPositionInfo (void) const { return _geoPositionInfo; } + void setPositionSource (QGCPositionSource source); + int updateInterval (void) const; + void setNmeaSourceDevice (QIODevice* device); - qreal gcsHeading() { return _gcsHeading; } + // Overrides from QGCTool + void setToolbox(QGCToolbox* toolbox) override; - void setPositionSource(QGCPositionSource source); - - int updateInterval() const; - - void setToolbox(QGCToolbox* toolbox); - - void setNmeaSourceDevice(QIODevice* device); private slots: void _positionUpdated(const QGeoPositionInfo &update); @@ -57,9 +56,10 @@ signals: void positionInfoUpdated(QGeoPositionInfo update); private: - int _updateInterval; - QGeoCoordinate _gcsPosition; - qreal _gcsHeading; + int _updateInterval; + QGeoPositionInfo _geoPositionInfo; + QGeoCoordinate _gcsPosition; + qreal _gcsHeading; QGeoPositionInfoSource* _currentSource; QGeoPositionInfoSource* _defaultSource; diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc index da8157b..6763c84 100644 --- a/src/PositionManager/SimulatedPosition.cc +++ b/src/PositionManager/SimulatedPosition.cc @@ -12,28 +12,30 @@ #include #include "SimulatedPosition.h" - -SimulatedPosition::simulated_motion_s SimulatedPosition::_simulated_motion[5] = {{0,250},{0,0},{0, -250},{-250, 0},{0,0}}; +#include "QGCApplication.h" +#include "MultiVehicleManager.h" SimulatedPosition::SimulatedPosition() - : QGeoPositionInfoSource(nullptr), - lat_int(47.3977420*1e7), - lon_int(8.5455941*1e7), - _step_cnt(0), - _simulate_motion_index(0), - _simulate_motion(true), - _rotation(0.0F) + : QGeoPositionInfoSource(nullptr) { - QDateTime currentDateTime = QDateTime::currentDateTime(); + _updateTimer.setSingleShot(false); + + // Initialize position to normal PX4 Gazebo home position + _lastPosition.setTimestamp(QDateTime::currentDateTime()); + _lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488)); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec); + _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec); - qsrand(currentDateTime.toTime_t()); + // When a vehicle shows up we switch location to the vehicle home position + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded); - connect(&update_timer, &QTimer::timeout, this, &SimulatedPosition::updatePosition); + connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition); } QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const { - return lastPosition; + return _lastPosition; } SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const @@ -41,24 +43,14 @@ SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMet return AllPositioningMethods; } -int SimulatedPosition::minimumUpdateInterval() const +void SimulatedPosition::startUpdates(void) { - return 1000; + _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval())); } -void SimulatedPosition::startUpdates() -{ - int interval = updateInterval(); - if (interval < minimumUpdateInterval()) - interval = minimumUpdateInterval(); - - update_timer.setSingleShot(false); - update_timer.start(interval); -} - -void SimulatedPosition::stopUpdates() +void SimulatedPosition::stopUpdates(void) { - update_timer.stop(); + _updateTimer.stop(); } void SimulatedPosition::requestUpdate(int /*timeout*/) @@ -66,53 +58,17 @@ void SimulatedPosition::requestUpdate(int /*timeout*/) emit updateTimeout(); } -int SimulatedPosition::getRandomNumber(int size) -{ - if(size == 0) { - return 0; - } - - int num = (qrand()%2 > 1) ? -1 : 1; - - return num*qrand()%size; -} - -void SimulatedPosition::updatePosition() +void SimulatedPosition::_updatePosition(void) { - int32_t lat_mov = 0; - int32_t lon_mov = 0; - - _rotation += (float) .1; - - if(!(_step_cnt++%30)) { - _simulate_motion_index++; - if(_simulate_motion_index > 4) { - _simulate_motion_index = 0; - } - } + int intervalMsecs = _updateTimer.interval(); - lat_mov = _simulated_motion[_simulate_motion_index].lat; - lon_mov = _simulated_motion[_simulate_motion_index].lon*sin(_rotation); + QGeoCoordinate coord = _lastPosition.coordinate(); + double horizontalDistance = _horizontalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); + double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); - lon_int += lat_mov; - lat_int += lon_mov; + _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance)); - double latitude = ((double) (lat_int + getRandomNumber(250)))*1e-7; - double longitude = ((double) (lon_int + getRandomNumber(250)))*1e-7; - - QDateTime timestamp = QDateTime::currentDateTime(); - - QGeoCoordinate position(latitude, longitude); - QGeoPositionInfo info(position, timestamp); - - if(lat_mov || lon_mov) { - info.setAttribute(QGeoPositionInfo::Attribute::Direction, 3.14/2); - info.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, 5); - } - - lastPosition = info; - - emit positionUpdated(info); + emit positionUpdated(_lastPosition); } QGeoPositionInfoSource::Error SimulatedPosition::error() const @@ -120,4 +76,21 @@ QGeoPositionInfoSource::Error SimulatedPosition::error() const return QGeoPositionInfoSource::NoError; } +void SimulatedPosition::_vehicleAdded(Vehicle* vehicle) +{ + if (vehicle->homePosition().isValid()) { + _lastPosition.setCoordinate(vehicle->homePosition()); + } else { + connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged); + } +} + +void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition) +{ + Vehicle* vehicle = qobject_cast(sender()); + if (homePosition.isValid()) { + _lastPosition.setCoordinate(homePosition); + disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged); + } +} diff --git a/src/PositionManager/SimulatedPosition.h b/src/PositionManager/SimulatedPosition.h index ebd3665..0baede0 100644 --- a/src/PositionManager/SimulatedPosition.h +++ b/src/PositionManager/SimulatedPosition.h @@ -7,13 +7,14 @@ * ****************************************************************************/ - #pragma once #include #include "QGCToolbox.h" #include +class Vehicle; + class SimulatedPosition : public QGeoPositionInfoSource { Q_OBJECT @@ -21,42 +22,28 @@ class SimulatedPosition : public QGeoPositionInfoSource public: SimulatedPosition(); - QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const; + QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const override; - PositioningMethods supportedPositioningMethods() const; - int minimumUpdateInterval() const; - Error error() const; + PositioningMethods supportedPositioningMethods (void) const override; + int minimumUpdateInterval (void) const override { return _updateIntervalMsecs; } + Error error (void) const override; public slots: - virtual void startUpdates(); - virtual void stopUpdates(); - - virtual void requestUpdate(int timeout = 5000); + void startUpdates (void) override; + void stopUpdates (void) override; + void requestUpdate (int timeout = 5000) override; private slots: - void updatePosition(); + void _updatePosition (void); + void _vehicleAdded (Vehicle* vehicle); + void _vehicleHomePositionChanged (QGeoCoordinate homePosition); private: - QTimer update_timer; - - QGeoPositionInfo lastPosition; - - // items for simulating QGC movement in jMAVSIM - - int32_t lat_int; - int32_t lon_int; - - struct simulated_motion_s { - int lon; - int lat; - }; - - static simulated_motion_s _simulated_motion[5]; - - int getRandomNumber(int size); - int _step_cnt; - int _simulate_motion_index; + QTimer _updateTimer; + QGeoPositionInfo _lastPosition; - bool _simulate_motion; - float _rotation; + static constexpr int _updateIntervalMsecs = 1000; + static constexpr double _horizontalVelocityMetersPerSec = 0.5; + static constexpr double _verticalVelocityMetersPerSec = 0.1; + static constexpr double _heading = 45; }; diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 6425246..d17b97b 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -162,6 +162,8 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) , _runningUnitTests (unitTesting) { _app = this; + _msecsElapsedTime.start(); + #ifdef Q_OS_LINUX #ifndef __mobile__ if (!_runningUnitTests) { diff --git a/src/QGCApplication.h b/src/QGCApplication.h index c48edcb..0e631dc 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -22,6 +22,7 @@ #include #include #include +#include #include "LinkConfiguration.h" #include "LinkManager.h" @@ -98,6 +99,8 @@ public: void setLanguage(); QQuickItem* mainRootWindow(); + uint64_t msecsSinceBoot(void) { return _msecsElapsedTime.elapsed(); } + public slots: /// You can connect to this slot to show an information message box from a different thread. void informationMessageBoxOnMainThread(const QString& title, const QString& msg); @@ -192,6 +195,7 @@ private: QTranslator _QGCTranslatorQt; QLocale _locale; bool _error = false; + QElapsedTimer _msecsElapsedTime; static const char* _settingsVersionKey; ///< Settings key which hold settings version static const char* _deleteAllSettingsKey; ///< If this settings key is set on boot, all settings will be deleted diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index eaa88ac..7dabbee 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -212,7 +212,7 @@ "type": "uint32", "enumStrings": "Never,Always,When in Follow Me Flight Mode", "enumValues": "0,1,2", - "defaultValue": 0 + "defaultValue": 2 }, { "name": "apmStartMavlinkStreams", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8eed1b3..68c1674 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -28,7 +28,6 @@ #include "ParameterManager.h" #include "QGCApplication.h" #include "QGCImageProvider.h" -#include "FollowMe.h" #include "MissionCommandTree.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" @@ -247,9 +246,6 @@ Vehicle::Vehicle(LinkInterface* link, _commonInit(); _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this); - // connect this vehicle to the follow me handle manager - connect(this, &Vehicle::flightModeChanged,_toolbox->followMe(), &FollowMe::followMeHandleManager); - // PreArm Error self-destruct timer connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout); _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); @@ -3685,6 +3681,11 @@ QString Vehicle::takeControlFlightMode(void) const return _firmwarePlugin->takeControlFlightMode(); } +QString Vehicle::followFlightMode(void) const +{ + return _firmwarePlugin->followFlightMode(); +} + QString Vehicle::vehicleImageOpaque() const { if(_firmwarePlugin) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index cde8f58..1407941 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -607,6 +607,7 @@ public: Q_PROPERTY(bool supportsSmartRTL READ supportsSmartRTL CONSTANT) Q_PROPERTY(QString landFlightMode READ landFlightMode CONSTANT) Q_PROPERTY(QString takeControlFlightMode READ takeControlFlightMode CONSTANT) + Q_PROPERTY(QString followFlightMode READ followFlightMode CONSTANT) Q_PROPERTY(QString firmwareTypeString READ firmwareTypeString NOTIFY firmwareTypeChanged) Q_PROPERTY(QString vehicleTypeString READ vehicleTypeString NOTIFY vehicleTypeChanged) Q_PROPERTY(QString vehicleImageOpaque READ vehicleImageOpaque CONSTANT) @@ -942,6 +943,7 @@ public: bool supportsSmartRTL () const; QString landFlightMode () const; QString takeControlFlightMode () const; + QString followFlightMode () const; double defaultCruiseSpeed () const { return _defaultCruiseSpeed; } double defaultHoverSpeed () const { return _defaultHoverSpeed; } QString firmwareTypeString () const; diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index 320c4a4..5229579 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -78,6 +78,7 @@ void BluetoothLink::_writeBytes(const QByteArray bytes) { if (_targetSocket) { if(_targetSocket->write(bytes) > 0) { + emit bytesSent(this, bytes); _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); } else { qWarning() << "Bluetooth write error"; diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index e04ecc4..2af1f66 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -182,6 +182,16 @@ signals: void bytesReceived(LinkInterface* link, QByteArray data); /** + * @brief New data has been sent + * * + * The new data is contained in the QByteArray data. + * The data is logged into telemetry logging system + * + * @param data the new bytes + */ + void bytesSent(LinkInterface* link, QByteArray data); + + /** * @brief This signal is emitted instantly when the link is connected **/ void connected(); diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index caa4d09..7ca89eb 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -199,6 +199,7 @@ void LinkManager::_addLink(LinkInterface* link) connect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); + connect(link, &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); _mavlinkProtocol->resetMetadataForLink(link); _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index f5a52d1..5e43702 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -160,6 +160,38 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) } /** + * This method parses all outcoming bytes and log a MAVLink packet. + * @param link The interface to read from + * @see LinkInterface + **/ + +void MAVLinkProtocol::logSentBytes(LinkInterface* link, QByteArray b){ + + uint8_t bytes_time[sizeof(quint64)]; + + Q_UNUSED(link); + if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { + + quint64 time = static_cast(QDateTime::currentMSecsSinceEpoch() * 1000); + + qToBigEndian(time,bytes_time); + + b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time))); + + int len = b.count(); + + if(_tempLogFile.write(b) != len) + { + // If there's an error logging data, raise an alert and stop logging. + emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName())); + _stopLogging(); + _logSuspendError = true; + } + } + +} + +/** * This method parses all incoming bytes and constructs a MAVLink packet. * It can handle multiple links in parallel, as each link has it's own buffer/ * parsing state machine. diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 7614ee6..06a155a 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -81,6 +81,9 @@ public: public slots: /** @brief Receive bytes from a communication interface */ void receiveBytes(LinkInterface* link, QByteArray b); + + /** @brief Log bytes sent from a communication interface */ + void logSentBytes(LinkInterface* link, QByteArray b); /** @brief Set the system id of this application */ void setSystemId(int id); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 46e5bfe..3625ac5 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -84,6 +84,7 @@ bool SerialLink::_isBootloader() void SerialLink::_writeBytes(const QByteArray data) { if(_port && _port->isOpen()) { + emit bytesSent(this, data); _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); _port->write(data); } else { diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index dddbf76..82dd895 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -81,6 +81,7 @@ void TCPLink::_writeBytes(const QByteArray data) if (_socket) { _socket->write(data); + emit bytesSent(this, data); _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } } diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 1642b9d..19db308 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -169,6 +169,7 @@ void UDPLink::_writeBytes(const QByteArray data) if (!_socket) { return; } + emit bytesSent(this, data); // Send to all manually targeted systems for(UDPCLient* target: _udpConfig->targetHosts()) { // Skip it if it's part of the session clients below