Better simulated position for Follow Me testing

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)

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<double>(intervalMsecs));
+    double          verticalDistance =      _verticalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
 
-    lastPosition = info;
+    _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance));
 
-    emit positionUpdated(info);
+    emit positionUpdated(_lastPosition);
 }
 
 QGeoPositionInfoSource::Error SimulatedPosition::error() const

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;
 };