Merge pull request #7066 from DonLakeFlyer/OrbitTelemetry

Show Orbit telemetry on map

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

src/MissionManager/QGCMapCircleVisuals.qml

@@ -105,8 +105,7 @@ Item {
         id: rotationIndicatorComponent
 
         MapQuickItem {
-            z:          QGroundControl.zOrderMapItems + 2
-            visible:    mapCircle.showRotation
+            visible: mapCircle.showRotation
 
             property bool topIndicator: true
 

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<double>(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<double>(orbitStatus.x) / qPow(10.0, 7.0), static_cast<double>(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;

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<QString, QTime> _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;