From 4bd1390c7ab20f4e8e4afddcc0d48ecd184d89b4 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
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 7ee772e..de45dcc 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 3e233ca..0e2ab4c 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<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;
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index c3076d5..fc81a39 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<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;