From e39c007890281ff89d4093b1d51f8d4fdbbf044f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 22 Sep 2019 10:40:51 -0700 Subject: [PATCH] Better simulated position for Follow Me testing --- src/PositionManager/PositionManager.cpp | 9 ++- src/PositionManager/SimulatedPosition.cc | 91 ++++++------------------ src/PositionManager/SimulatedPosition.h | 42 ++++------- 3 files changed, 38 insertions(+), 104 deletions(-) diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 38d36d0b1..16629b376 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/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) diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc index da8157b21..4a307a2ea 100644 --- a/src/PositionManager/SimulatedPosition.cc +++ b/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(intervalMsecs)); + double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast(intervalMsecs)); - lastPosition = info; + _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance)); - emit positionUpdated(info); + emit positionUpdated(_lastPosition); } QGeoPositionInfoSource::Error SimulatedPosition::error() const diff --git a/src/PositionManager/SimulatedPosition.h b/src/PositionManager/SimulatedPosition.h index ebd36657e..b64c243d2 100644 --- a/src/PositionManager/SimulatedPosition.h +++ b/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; }; -- 2.22.0