Heartbeat handling added to WimaController

qgroundcontrol.pro

@@ -432,6 +432,7 @@ HEADERS += \
     src/Wima/Geometry/GeoPoint3D.h \
     src/Wima/Geometry/Polygon2D.h \
     src/Wima/Geometry/PolygonArray.h \
+    src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QtROSJsonFactory.h \
     src/Wima/Snake/QtROSTypeFactory.h \

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -244,19 +244,15 @@ FlightMap {
     // Add Snake tiles center points to the map
     MapItemView {
 
-        property bool _lengthMatching: wimaController.snakeTileCenterPoints.count
-                                       > 0
-
         property bool _enable:    wimaController.enableWimaController.value
                                && wimaController.enableSnake.value
-                               && _lengthMatching
 
         model: _enable ? wimaController.snakeTileCenterPoints : 0
 
         delegate: MapCircle{
             center:         modelData
             border.color:   "transparent"
-            color:          getColor(wimaController.snakeProgress[index])
+            color:          getColor(wimaController.nemoProgress[index])
             radius:         0.6
             opacity:        1
             z:              1

src/FlightDisplay/FlightDisplayWimaMenu.qml

@@ -204,10 +204,9 @@ Item {
 
                             // Snake connection status.
                             QGCLabel {
-                                property int _connectionStatus: wimaController.snakeConnectionStatus
-                                text: _connectionStatus === 1 ?
-                                          qsTr("Status: Connected")
-                                        : qsTr("Status: Not Connected")
+                                property int index: wimaController.nemoStatus
+                                property string statusString: wimaController.nemoStatusString
+                                text: "Status: " + statusString
                                 Layout.fillWidth: true
                             }
 
@@ -370,7 +369,7 @@ Item {
                         QGCButton {
                             id: buttonSmartRTL
                             text:               qsTr("Smart RTL")
-                            onClicked:          wimaController.initSmartRTL();
+                            onClicked:          wimaController.requestSmartRTL();
                             Layout.columnSpan:  2
                             Layout.fillWidth: true
                         }

src/Wima/Snake/QNemoHeartbeat.h

+#pragma once
+
+#include "ros_bridge/include/GenericMessages.h"
+
+using QNemoHeartbeat = ROSBridge::GenericMessages::NemoMsgs::Heartbeat;

src/Wima/WaypointManager/RTLManager.cpp

@@ -81,22 +81,28 @@ bool WaypointManager::RTLManager::reset()
     return true;
 }
 
-QString WaypointManager::RTLManager::checkPrecondition()
+bool WaypointManager::RTLManager::checkPrecondition(QString &errorString)
 {
+    // Be aware of the error message order!
     Vehicle *managerVehicle = _settings->masterController()->managerVehicle();
-    if (!managerVehicle->flying()) {
-        return QString("Vehicle is not flying. Smart RTL not available.");
+
+    if ( managerVehicle->isOfflineEditingVehicle() ) {
+        errorString.append("No vehicle connected. Smart RTL not available.");
+        return false;
     }
 
-    if (!managerVehicle->isOfflineEditingVehicle()) {
-        return QString("Not connected to any vehicle. Smart RTL not available.");
+    if ( !managerVehicle->flying() ) {
+        errorString.append("Vehicle is not flying. Smart RTL not available.");
+        return false;
     }
 
-    if (!_areaInterface->joinedArea()->containsCoordinate(managerVehicle->coordinate())) {
-        return QString("Vehicle not inside save area. Smart RTL not available.");
+    if ( !_areaInterface->joinedArea()->containsCoordinate(
+             managerVehicle->coordinate()) ) {
+        errorString.append("Vehicle not inside save area. Smart RTL not available.");
+        return false;
     }
 
-    return QString();
+    return true;
 }
 
 bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,

src/Wima/WaypointManager/RTLManager.h

@@ -76,7 +76,7 @@ public:
     //!
     virtual bool reset() override;
 
-    QString checkPrecondition();
+    bool checkPrecondition(QString &errorString);
 
 protected:
     bool _insertMissionItem(const QGeoCoordinate &c,

src/Wima/WimaController.h

@@ -33,12 +33,15 @@
 #include "Snake/SnakeTilesLocal.h"
 #include "Geometry/GeoPoint3D.h"
 #include "Snake/QNemoProgress.h"
+#include "Snake/QNemoHeartbeat.h"
 
 #include "ros_bridge/include/ROSBridge.h"
 
 #include "WaypointManager/DefaultManager.h"
 #include "WaypointManager/RTLManager.h"
 
+#include <map>
+
 #define CHECK_BATTERY_INTERVAL 1000 // ms
 #define SMART_RTL_MAX_ATTEMPTS 3 // times
 #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
@@ -60,8 +63,6 @@ class WimaController : public QObject
 
 public:
 
-    enum SnakeConnectionStatus {Connected = 1, NotConnected = 0};
-
 
     WimaController(QObject *parent = nullptr);
 
@@ -149,9 +150,13 @@ public:
                READ     enableSnake
                CONSTANT
                )
-    Q_PROPERTY(long     snakeConnectionStatus
-               READ     snakeConnectionStatus
-               NOTIFY   snakeConnectionStatusChanged
+    Q_PROPERTY(int      nemoStatus
+               READ     nemoStatus
+               NOTIFY   nemoStatusChanged
+               )
+    Q_PROPERTY(QString  nemoStatusString
+               READ     nemoStatusString
+               NOTIFY   nemoStatusStringChanged
                )
     Q_PROPERTY(bool     snakeCalcInProgress
                READ     snakeCalcInProgress
@@ -224,8 +229,9 @@ public:
     // Snake data.
     QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
     QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
-    QVector<int>                nemoProgress           (void)           { return _nemoProgress.progress();}    
-    long                        snakeConnectionStatus  (void) const;
+    QVector<int>                nemoProgress           (void);
+    int                         nemoStatus             (void) const;
+    QString                     nemoStatusString       (void) const;
     bool                        snakeCalcInProgress    (void) const;
 
     // Smart RTL.
@@ -303,6 +309,8 @@ signals:
     void snakeTilesChanged                  (void);
     void snakeTileCenterPointsChanged       (void);
     void nemoProgressChanged                (void);
+    void nemoStatusChanged                  (void);
+    void nemoStatusStringChanged            (void);
 
 private slots:
 
@@ -325,11 +333,10 @@ private slots:
     void _setPhaseDuration  (double duration);
     // SMART RTL
     void    _checkBatteryLevel                  (void);
-    QString _checkSmartRTLPreCondition          (void);
+    bool    _checkSmartRTLPreCondition          (QString &errorString);
     void    _initSmartRTL                       ();
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
-    void _setSnakeConnectionStatus           (SnakeConnectionStatus status);
     void _setSnakeCalcInProgress             (bool inProgress);
     bool _isScenarioDefined                  (void);
     bool _isScenarioDefinedErrorMessage      (void);
@@ -343,6 +350,7 @@ private slots:
     void _switchWaypointManager(WaypointManager::ManagerBase &manager);
 
 private:
+    using StatusMap = std::map<int, QString>;
     // Snake.
     void _progressFromJson                   (JsonDocUPtr pDoc,
                                               QNemoProgress &progress);
@@ -397,7 +405,6 @@ private:
 //    double                       _phaseDuration; // the phase duration in seconds
 
     // Snake
-    SnakeConnectionStatus   _snakeConnectionStatus;
     bool                    _snakeCalcInProgress;
     bool                    _snakeRecalcNecessary;
     bool                    _scenarioDefinedBool;
@@ -408,7 +415,10 @@ private:
     SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
     QVariantList            _snakeTileCenterPoints;
     QNemoProgress           _nemoProgress; // measurement progress
+    QNemoHeartbeat          _nemoHeartbeat; // measurement progress
+    int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
+    static StatusMap        _nemoStatusMap;
 
     // Periodic tasks.
     QTimer                  _eventTimer;

src/comm/ros_bridge/include/GenericMessages.h

@@ -343,6 +343,29 @@ protected:
 
 typedef GenericProgress<> Progress;
 
+// ==============================================================================
+// class Heartbeat
+// ==============================================================================
+
+//! @brief C++ representation of nemo_msgs/Heartbeat message
+class Heartbeat : public MessageBaseClass{
+public:
+    typedef MessageGroups::HeartbeatGroup Group;
+    Heartbeat(){}
+    Heartbeat(int status) :_status(status){}
+    Heartbeat(const Heartbeat &heartbeat) :_status(heartbeat.status()){}
+    ~Heartbeat() = default;
+
+    virtual Heartbeat *Clone() const override { return new Heartbeat(*this); }
+
+    virtual int     status      (void)const    {return _status;}
+    virtual void    setStatus   (int status)    {_status = status;}
+protected:
+    int _status;
+};
+
+typedef GenericProgress<> Progress;
+
 } // namespace NemoMsgs
 } // namespace GenericMessages
 } // namespace ROSBridge

src/comm/ros_bridge/include/JsonFactory.h

@@ -186,6 +186,20 @@ private:
 
         return doc;
     }
+
+    // ===========================================================================
+    // HeartbeatGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::HeartbeatGroup>){
+        using namespace ROSBridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = JsonMethodes::NemoMsgs::Heartbeat::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
 };
 
 class StdHeaderPolicy

src/comm/ros_bridge/include/JsonMethodes.h

@@ -643,7 +643,7 @@ namespace Progress {
     {
         rapidjson::Value progressJson(rapidjson::kArrayType);
         for(unsigned long i=0; i < std::uint64_t(p.progress().size()); ++i){
-            progressJson.PushBack(rapidjson::Value().SetInt(std::int8_t(p.progress()[i])), allocator);
+            progressJson.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
         }
         value.AddMember("progress", progressJson, allocator);
         return true;
@@ -662,10 +662,32 @@ namespace Progress {
         p.progress().clear();
         p.progress().reserve(sz);
         for (unsigned long i=0; i < sz; ++i)
-            p.progress().push_back(std::int8_t(jsonProgress[i].GetInt()));
+            p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
         return true;
     }
 } // namespace Progress
+
+//! @brief Namespace containing methodes for nemo_msgs/Heartbeat generation.
+namespace Heartbeat {
+    template <class HeartbeatType>
+    bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+    {
+        value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
+        return true;
+    }
+
+    template <class HeartbeatType>
+    bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
+    {
+        if (!value.HasMember("status") || !value["status"].IsInt()){
+            assert(false);
+            return false;
+        }
+
+        heartbeat.setStatus(value["status"].GetInt());
+        return true;
+    }
+} // namespace Heartbeat
 } // namespace NemoMsgs
 } // namespace JsonMethodes
 } // namespace ROSBridge

src/comm/ros_bridge/include/MessageGroups.h

@@ -98,11 +98,15 @@ struct NemoMsgs {
 
     //!
     //! \brief The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
-    //!
-    //! The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
     struct ProgressGroup {
         static StringType label() {return "Progress";}
     };
+
+    //!
+    //! \brief The HeartbeatGroup struct is used the mark a class as a ROS nemo_msgs/Heartbeat message.
+    struct HeartbeatGroup {
+        static StringType label() {return "Heartbeat";}
+    };
 };
 
 
@@ -134,6 +138,10 @@ typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
                                     MessageGroups::NemoMsgs::ProgressGroup>
         ProgressGroup;
 
+typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
+                                    MessageGroups::NemoMsgs::HeartbeatGroup>
+        HeartbeatGroup;
+
 } // end namespace MessageGroups
 
 } // end namespace ros_bridge

src/comm/ros_bridge/include/TypeFactory.h

@@ -120,6 +120,17 @@ private:
         return ret;
     }
 
+    // ===========================================================================
+    // HeartbeatGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::HeartbeatGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::NemoMsgs::Heartbeat::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
 
 };
 

src/comm/utilities.h

@@ -21,25 +21,25 @@ public:
     //!
     //! \brief EventTicker Constructor used for initilization. The function \fn must not be called if this constructor is used.
     //! \param timerInterval The timer interval of the timer provided by the user.
-    //! \param tickInterval The thick intervall (timerInterval <= tickInterval), see \fn ready().
+    //! \param targetTime The thick intervall (timerInterval <= targetTime), see \fn ready().
     //!
-    EventTicker(double timerInterval, double tickInterval) {
+    EventTicker(double timerInterval, double targetTime) {
         _isInitialized = false;
-        init(timerInterval, tickInterval);
+        init(timerInterval, targetTime);
     }
 
     //!
     //! \brief init Used to init the EventTicker.
     //! \param timerInterval The timer interval of the timer provided by the user.
-    //! \param tickInterval The thick intervall (timerInterval <= tickInterval), see \fn ready().
+    //! \param targetTime The thick intervall (timerInterval <= targetTime), see \fn ready().
     //!
-    void init(double timerInterval, double tickInterval){
+    void init(double timerInterval, double targetTime){
         assert(_isInitialized == false);
         assert(timerInterval > 0);
-        assert(tickInterval >= timerInterval);
+        assert(targetTime >= timerInterval);
         _timerInterval  = timerInterval;
-        _tickInerval    = tickInterval;
-        _tickCount      = 0;
+        _targetTime    = targetTime;
+        _ticks      = 0;
         _isInitialized  = true;
     }
 
@@ -48,22 +48,28 @@ public:
     //! \return True if the event is ready to be executed, false either.
     //!
     //! This function must be called by a user provided timer interrupt handler. A internal counter increases with every call.
-    //! If _tickInerval <= _timerInterval*_tickCount the counter gets reset and the function returns true. If the condition is not
+    //! If _targetTime <= _timerInterval*_ticks the counter gets reset and the function returns true. If the condition is not
     //! met the function returns false.
     //!
     bool ready(void) {
         assert(_isInitialized == true);
-        _tickCount++;
-        bool ready = _tickInerval <= _timerInterval*_tickCount;
+        _ticks++;
+        bool ready = _targetTime <= _timerInterval*_ticks;
         if (ready)
-            _tickCount = 0;
+            _ticks = 0;
         return ready;
     }
 
+    //!
+    //! \brief Resets the tick count.
+    void reset(void) {
+        _ticks = 0;
+    }
+
 private:
     double          _timerInterval;
-    double          _tickInerval;
-    unsigned long   _tickCount;
+    double          _targetTime;
+    unsigned long   _ticks;
     bool            _isInitialized;
 };