From 3536f1af9d5b9fd5696a0a9a4de957aee5f4b0aa Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Wed, 21 Aug 2019 14:15:01 +1200 Subject: [PATCH 01/13] Add oucoming messages in log To have both order from the GCS and drone messages in logs So that it's possible to analyze the complete exchange between QGC and the drone. - Add a signal emitted by writeBytes of each LinkInterface - Connect this signal to MAVLinkProtocol::logSentBytes in LinkManager - MAVLinkProtocol::logSentBytes will parse and log messages sent from QGC --- src/comm/LinkInterface.h | 10 ++++++++++ src/comm/LinkManager.cc | 1 + src/comm/MAVLinkProtocol.cc | 46 +++++++++++++++++++++++++++++++++++++++++++++ src/comm/MAVLinkProtocol.h | 3 +++ src/comm/TCPLink.cc | 1 + src/comm/UDPLink.cc | 1 + 6 files changed, 62 insertions(+) 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..e1a159d 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -160,6 +160,52 @@ 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 mavlinkChannel = link->mavlinkChannel(); + static mavlink_message_t _sent_message; + + for (int position = 0; position < b.size(); position++) { + + if(mavlink_parse_char(mavlinkChannel, static_cast(b[position]), &_sent_message, &_status)){ + + if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)]; + + // Write the uint64 time in microseconds in big endian format before the message. + // This timestamp is saved in UTC time. We are only saving in ms precision because + // getting more than this isn't possible with Qt without a ton of extra code. + quint64 time = static_cast(QDateTime::currentMSecsSinceEpoch() * 1000); + qToBigEndian(time, buf); + + // Then write the message to the buffer + int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_sent_message); + + // Determine how many bytes were written by adding the timestamp size to the message size + len += sizeof(quint64); + + // Now write this timestamp/message pair to the log. + QByteArray b(reinterpret_cast(buf), len); + + 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/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 From babe2b7c422b5b0a94b94dc6f8c56acd0b3e0a28 Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Mon, 2 Sep 2019 11:54:46 +1200 Subject: [PATCH 02/13] Simplify logSendBytes Remove the use of mavlink_parse_char which distorts channel stats --- src/comm/MAVLinkProtocol.cc | 40 ++++++++++++---------------------------- 1 file changed, 12 insertions(+), 28 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index e1a159d..073fe55 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -167,40 +167,24 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) void MAVLinkProtocol::logSentBytes(LinkInterface* link, QByteArray b){ - uint8_t mavlinkChannel = link->mavlinkChannel(); - static mavlink_message_t _sent_message; + uint8_t bytes_time[sizeof(quint64)]; - for (int position = 0; position < b.size(); position++) { + Q_UNUSED(link); - if(mavlink_parse_char(mavlinkChannel, static_cast(b[position]), &_sent_message, &_status)){ + quint64 time = static_cast(QDateTime::currentMSecsSinceEpoch() * 1000); - if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)]; - - // Write the uint64 time in microseconds in big endian format before the message. - // This timestamp is saved in UTC time. We are only saving in ms precision because - // getting more than this isn't possible with Qt without a ton of extra code. - quint64 time = static_cast(QDateTime::currentMSecsSinceEpoch() * 1000); - qToBigEndian(time, buf); + qToBigEndian(time,bytes_time); - // Then write the message to the buffer - int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_sent_message); + b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time))); - // Determine how many bytes were written by adding the timestamp size to the message size - len += sizeof(quint64); + int len = b.count(); - // Now write this timestamp/message pair to the log. - QByteArray b(reinterpret_cast(buf), len); - - 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; - } - } - } + 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; } } From e51aeaefda200370eb3799cb198f7f4a6061251b Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Fri, 6 Sep 2019 10:55:03 +1200 Subject: [PATCH 03/13] Re-add remove if statement Add again test to insure log is not suspend and tempfile log is open --- src/comm/MAVLinkProtocol.cc | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 073fe55..5e43702 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -170,21 +170,23 @@ 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); + quint64 time = static_cast(QDateTime::currentMSecsSinceEpoch() * 1000); - qToBigEndian(time,bytes_time); + qToBigEndian(time,bytes_time); - b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time))); + b.insert(0,QByteArray((const char*)bytes_time,sizeof(bytes_time))); - int len = b.count(); + 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; + 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; + } } } From 1c303eb5aee474a91d51c96e177cae313a52a18f Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Mon, 9 Sep 2019 11:28:01 +1200 Subject: [PATCH 04/13] Add Bluetooth and Serial to outcoming logs --- src/comm/BluetoothLink.cc | 1 + src/comm/SerialLink.cc | 1 + 2 files changed, 2 insertions(+) diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index 320c4a4..e0120bc 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, data); _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); } else { qWarning() << "Bluetooth write error"; 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 { From 798972b40c7f1afc6841a0db5a26e9b5bb769c68 Mon Sep 17 00:00:00 2001 From: Pierre TILAK Date: Fri, 13 Sep 2019 09:09:44 +1200 Subject: [PATCH 05/13] Fix bluetoothLink typo --- src/comm/BluetoothLink.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index e0120bc..5229579 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -78,7 +78,7 @@ void BluetoothLink::_writeBytes(const QByteArray bytes) { if (_targetSocket) { if(_targetSocket->write(bytes) > 0) { - emit bytesSent(this, data); + emit bytesSent(this, bytes); _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); } else { qWarning() << "Bluetooth write error"; From f8e2dbe90e85bbbf63660703fe45648938abac7b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 21 Sep 2019 11:52:25 -0700 Subject: [PATCH 06/13] Plugin'ized follow me support --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 44 +++++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 5 +- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 7 +- src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h | 3 +- src/FirmwarePlugin/FirmwarePlugin.cc | 25 +++ src/FirmwarePlugin/FirmwarePlugin.h | 7 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/FlightDisplay/FlightDisplayViewMap.qml | 1 - src/FollowMe/FollowMe.cc | 228 ++++++++++------------ src/FollowMe/FollowMe.h | 61 +++--- src/PositionManager/PositionManager.cpp | 2 + src/PositionManager/PositionManager.h | 24 +-- src/QGCApplication.cc | 2 + src/QGCApplication.h | 4 + src/Settings/App.SettingsGroup.json | 2 +- src/Vehicle/Vehicle.cc | 9 +- src/Vehicle/Vehicle.h | 2 + 17 files changed, 249 insertions(+), 178 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 64a4962..1ef62b3 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.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index a675e74..f6dc886 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -64,9 +64,10 @@ 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; } diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index ad17a03..c71b395 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -45,7 +45,8 @@ 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; } 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.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 15beb71..0ab85df 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -80,7 +80,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..3ca922b 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(qMax(_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..38d36d0 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/src/PositionManager/PositionManager.cpp @@ -77,6 +77,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/QGCApplication.cc b/src/QGCApplication.cc index 0d0d45e..887b39e 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -161,6 +161,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 23bc05d..89e8065 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" @@ -245,9 +244,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); @@ -3699,6 +3695,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 2876044..97ac0af 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -606,6 +606,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; From b0581e29806410d6e65f44d2b2bf33eb768e7fc6 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 22 Sep 2019 09:39:21 -0700 Subject: [PATCH 07/13] Fix interval --- src/FollowMe/FollowMe.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index 3ca922b..2eb0f0e 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -61,7 +61,7 @@ void FollowMe::_settingsChanged() void FollowMe::_enableFollowSend() { if (!_gcsMotionReportTimer.isActive()) { - _gcsMotionReportTimer.setInterval(qMax(_toolbox->qgcPositionManager()->updateInterval(), 250)); + _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250)); _gcsMotionReportTimer.start(); } } From e39c007890281ff89d4093b1d51f8d4fdbbf044f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 22 Sep 2019 10:40:51 -0700 Subject: [PATCH 08/13] Better simulated position for Follow Me testing --- src/PositionManager/PositionManager.cpp | 9 ++-- src/PositionManager/SimulatedPosition.cc | 91 ++++++++------------------------ src/PositionManager/SimulatedPosition.h | 42 +++++---------- 3 files changed, 38 insertions(+), 104 deletions(-) diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 38d36d0..16629b3 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 0 setPositionSource(QGCPositionSource::InternalGPS); +#else + setPositionSource(QGCPositionManager::Simulated); +#endif } void QGCPositionManager::setNmeaSourceDevice(QIODevice* device) diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc index da8157b..4a307a2 100644 --- a/src/PositionManager/SimulatedPosition.cc +++ b/src/PositionManager/SimulatedPosition.cc @@ -13,27 +13,24 @@ #include "SimulatedPosition.h" -SimulatedPosition::simulated_motion_s SimulatedPosition::_simulated_motion[5] = {{0,250},{0,0},{0, -250},{-250, 0},{0,0}}; - 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); - qsrand(currentDateTime.toTime_t()); + // 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); - 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 +38,14 @@ SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMet return AllPositioningMethods; } -int SimulatedPosition::minimumUpdateInterval() const +void SimulatedPosition::startUpdates(void) { - return 1000; -} - -void SimulatedPosition::startUpdates() -{ - int interval = updateInterval(); - if (interval < minimumUpdateInterval()) - interval = minimumUpdateInterval(); - - update_timer.setSingleShot(false); - update_timer.start(interval); + _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval())); } -void SimulatedPosition::stopUpdates() +void SimulatedPosition::stopUpdates(void) { - update_timer.stop(); + _updateTimer.stop(); } void SimulatedPosition::requestUpdate(int /*timeout*/) @@ -66,53 +53,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; - } - } - - lat_mov = _simulated_motion[_simulate_motion_index].lat; - lon_mov = _simulated_motion[_simulate_motion_index].lon*sin(_rotation); - - lon_int += lat_mov; - lat_int += lon_mov; - - 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); + int intervalMsecs = _updateTimer.interval(); - if(lat_mov || lon_mov) { - info.setAttribute(QGeoPositionInfo::Attribute::Direction, 3.14/2); - info.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, 5); - } + QGeoCoordinate coord = _lastPosition.coordinate(); + double horizontalDistance = _horizontalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); + double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); - lastPosition = info; + _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance)); - emit positionUpdated(info); + emit positionUpdated(_lastPosition); } QGeoPositionInfoSource::Error SimulatedPosition::error() const diff --git a/src/PositionManager/SimulatedPosition.h b/src/PositionManager/SimulatedPosition.h index ebd3665..b64c243 100644 --- a/src/PositionManager/SimulatedPosition.h +++ b/src/PositionManager/SimulatedPosition.h @@ -21,42 +21,26 @@ 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(); 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; }; From e91b6d6a654bd3ffad9b4a98a6b833981407d300 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 11:22:51 -0700 Subject: [PATCH 09/13] Change to vehicle location when home position shows up --- src/PositionManager/SimulatedPosition.cc | 26 ++++++++++++++++++++++++-- src/PositionManager/SimulatedPosition.h | 7 +++++-- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc index 4a307a2..6763c84 100644 --- a/src/PositionManager/SimulatedPosition.cc +++ b/src/PositionManager/SimulatedPosition.cc @@ -12,6 +12,8 @@ #include #include "SimulatedPosition.h" +#include "QGCApplication.h" +#include "MultiVehicleManager.h" SimulatedPosition::SimulatedPosition() : QGeoPositionInfoSource(nullptr) @@ -25,7 +27,10 @@ SimulatedPosition::SimulatedPosition() _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec); _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec); - connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::updatePosition); + // When a vehicle shows up we switch location to the vehicle home position + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded); + + connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition); } QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const @@ -53,7 +58,7 @@ void SimulatedPosition::requestUpdate(int /*timeout*/) emit updateTimeout(); } -void SimulatedPosition::updatePosition(void) +void SimulatedPosition::_updatePosition(void) { int intervalMsecs = _updateTimer.interval(); @@ -71,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 b64c243..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 @@ -33,7 +34,9 @@ public slots: void requestUpdate (int timeout = 5000) override; private slots: - void updatePosition(); + void _updatePosition (void); + void _vehicleAdded (Vehicle* vehicle); + void _vehicleHomePositionChanged (QGeoCoordinate homePosition); private: QTimer _updateTimer; From d3b1e88d518e9ec330593c297b789efe02320b41 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 11:23:03 -0700 Subject: [PATCH 10/13] Turn off simulation --- src/PositionManager/PositionManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 16629b3..7d5fab0 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/src/PositionManager/PositionManager.cpp @@ -46,7 +46,7 @@ void QGCPositionManager::setToolbox(QGCToolbox *toolbox) } _simulatedSource = new SimulatedPosition(); -#if 0 +#if 1 setPositionSource(QGCPositionSource::InternalGPS); #else setPositionSource(QGCPositionManager::Simulated); From 94ab6fc92fdd88471b7b736e3301535c4d4b19eb Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 11:23:11 -0700 Subject: [PATCH 11/13] Finish up follow me support --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 2 +- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 6 +++++- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 1 + src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc | 5 +++++ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h | 1 + 5 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 1ef62b3..2f2f7a8 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -1088,7 +1088,7 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) { - if (vehicle->homePosition().isValid()) { + if (!vehicle->homePosition().isValid()) { static bool sentOnce = false; if (!sentOnce) { sentOnce = true; 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 f6dc886..08be16f 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -71,6 +71,7 @@ public: 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 c71b395..dea59ff 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -53,6 +53,7 @@ public: 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; From 1ef18399a31715ada12f69c3efd5db051ad5a2d0 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 11:26:15 -0700 Subject: [PATCH 12/13] Update --- ChangeLog.md | 1 + 1 file changed, 1 insertion(+) diff --git a/ChangeLog.md b/ChangeLog.md index 708c14d..5d5fc8f 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes. ### 3.6.0 - Daily Build +* 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 From 892a486a032e73cc8a2c95a773d436e966ff031f Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 14:07:21 -0700 Subject: [PATCH 13/13] Allow unknown flight modes without warning --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 2 -- 1 file changed, 2 deletions(-) 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;