From 4bd1390c7ab20f4e8e4afddcc0d48ecd184d89b4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 5 Dec 2018 18:42:30 -0800 Subject: [PATCH] Show orbit telemetry status on map --- src/FlightDisplay/FlightDisplayViewMap.qml | 7 ++++ src/Vehicle/Vehicle.cc | 44 +++++++++++++++++++++- src/Vehicle/Vehicle.h | 17 +++++++++ 3 files changed, 67 insertions(+), 1 deletion(-) diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 7ee772ef7..de45dcc04 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -354,6 +354,13 @@ FlightMap { } } + // Used to show orbit status telemetry from the vehicle + QGCMapCircleVisuals { + mapControl: parent + mapCircle: _activeVehicle.orbitMapCircle + visible: _activeVehicle ? _activeVehicle.orbitActive : false + } + // Handle guided mode clicks MouseArea { anchors.fill: parent diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 3e233ca83..0e2ab4cf8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -182,6 +182,7 @@ Vehicle::Vehicle(LinkInterface* link, , _uid(0) , _lastAnnouncedLowBatteryPercent(100) , _priorityLinkCommanded(false) + , _orbitActive(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -282,6 +283,8 @@ Vehicle::Vehicle(LinkInterface* link, _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints); connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint); + connect(&_orbitTelemetryTimer, &QTimer::timeout, this, &Vehicle::_orbitTelemetryTimeout); + // Create camera manager instance _cameras = _firmwarePlugin->createCameraManager(this); emit dynamicCamerasChanged(); @@ -377,6 +380,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _gitHash(versionNotSetValue) , _uid(0) , _lastAnnouncedLowBatteryPercent(100) + , _orbitActive(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -772,6 +776,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_STATUSTEXT: _handleStatusText(message); break; + case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS: + _handleOrbitExecutionStatus(message); + break; case MAVLINK_MSG_ID_PING: _handlePing(link, message); @@ -803,7 +810,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } - #if !defined(NO_ARDUPILOT_DIALECT) void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) { @@ -817,6 +823,42 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) } #endif +void Vehicle::_handleOrbitExecutionStatus(const mavlink_message_t& message) +{ + mavlink_orbit_execution_status_t orbitStatus; + + mavlink_msg_orbit_execution_status_decode(&message, &orbitStatus); + + double newRadius = qAbs(static_cast(orbitStatus.radius)); + if (!qFuzzyCompare(_orbitMapCircle.radius()->rawValue().toDouble(), newRadius)) { + _orbitMapCircle.radius()->setRawValue(newRadius); + } + + bool newOrbitClockwise = orbitStatus.radius > 0 ? true : false; + if (_orbitMapCircle.clockwiseRotation() != newOrbitClockwise) { + _orbitMapCircle.setClockwiseRotation(newOrbitClockwise); + } + + QGeoCoordinate newCenter(static_cast(orbitStatus.x) / qPow(10.0, 7.0), static_cast(orbitStatus.y) / qPow(10.0, 7.0)); + if (_orbitMapCircle.center() != newCenter) { + _orbitMapCircle.setCenter(newCenter); + } + + if (!_orbitActive) { + _orbitActive = true; + _orbitMapCircle.setShowRotation(true); + emit orbitActiveChanged(true); + } + + _orbitTelemetryTimer.start(_orbitTelemetryTimeoutMsecs); +} + +void Vehicle::_orbitTelemetryTimeout(void) +{ + _orbitActive = false; + emit orbitActiveChanged(false); +} + void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message) { mavlink_camera_image_captured_t feedback; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c3076d568..fc81a396c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -20,6 +20,7 @@ #include "MAVLinkProtocol.h" #include "UASMessageHandler.h" #include "SettingsFact.h" +#include "QGCMapCircle.h" class UAS; class UASInterface; @@ -630,6 +631,10 @@ public: Q_PROPERTY(quint64 mavlinkLossCount READ mavlinkLossCount NOTIFY mavlinkStatusChanged) Q_PROPERTY(float mavlinkLossPercent READ mavlinkLossPercent NOTIFY mavlinkStatusChanged) + // The following properties relate to Orbit status + Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged) + Q_PROPERTY(QGCMapCircle* orbitMapCircle READ orbitMapCircle CONSTANT) + // Vehicle state used for guided control Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) ///< Vehicle is in landing pattern (DO_LAND_START) @@ -931,6 +936,9 @@ public: int telemetryRNoise () { return _telemetryRNoise; } bool autoDisarm (); bool highLatencyLink () const { return _highLatencyLink; } + bool orbitActive () const { return _orbitActive; } + QGCMapCircle* orbitMapCircle () { return &_orbitMapCircle; } + /// Get the maximum MAVLink protocol version supported /// @return the maximum version unsigned maxProtoVersion () const { return _maxProtoVersion; } @@ -1130,6 +1138,7 @@ signals: void sensorsEnabledBitsChanged (int sensorsEnabledBits); void sensorsHealthBitsChanged (int sensorsHealthBits); void sensorsUnhealthyBitsChanged(int sensorsUnhealthyBits); + void orbitActiveChanged (bool orbitActive); void firmwareVersionChanged(void); void firmwareCustomVersionChanged(void); @@ -1203,6 +1212,7 @@ private slots: void _trafficUpdate (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); void _adsbTimerTimeout (); + void _orbitTelemetryTimeout (void); private: bool _containsLink(LinkInterface* link); @@ -1241,6 +1251,7 @@ private: void _handleDistanceSensor(mavlink_message_t& message); void _handleEstimatorStatus(mavlink_message_t& message); void _handleStatusText(mavlink_message_t& message); + void _handleOrbitExecutionStatus(const mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -1443,6 +1454,12 @@ private: QMap _noisySpokenPrearmMap; ///< Used to prevent PreArm messages from being spoken too often + // Orbit status values + bool _orbitActive; + QGCMapCircle _orbitMapCircle; + QTimer _orbitTelemetryTimer; + static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive + // FactGroup facts Fact _rollFact; -- 2.22.0