Lorenz Meier
9 years ago
14 changed files with 441 additions and 130 deletions
@ -0,0 +1,88 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
#include "PositionManager.h" |
||||||
|
|
||||||
|
QGCPositionManager::QGCPositionManager(QGCApplication* app) : |
||||||
|
QGCTool(app), |
||||||
|
_updateInterval(0), |
||||||
|
_currentSource(nullptr) |
||||||
|
{ |
||||||
|
_defaultSource = QGeoPositionInfoSource::createDefaultSource(this); |
||||||
|
_simulatedSource = new SimulatedPosition(); |
||||||
|
|
||||||
|
// if the default source is not availble for whatever reason
|
||||||
|
// fall back to a simulated source
|
||||||
|
|
||||||
|
if(_defaultSource == nullptr) { |
||||||
|
_defaultSource = _simulatedSource; |
||||||
|
} |
||||||
|
|
||||||
|
setPositionSource(QGCPositionSource::GPS); |
||||||
|
} |
||||||
|
|
||||||
|
QGCPositionManager::~QGCPositionManager() |
||||||
|
{ |
||||||
|
delete(_simulatedSource); |
||||||
|
} |
||||||
|
|
||||||
|
void QGCPositionManager::positionUpdated(const QGeoPositionInfo &update) |
||||||
|
{ |
||||||
|
|
||||||
|
QGeoCoordinate position(update.coordinate().latitude(), update.coordinate().longitude()); |
||||||
|
|
||||||
|
emit lastPositionUpdated(update.isValid(), QVariant::fromValue(position)); |
||||||
|
emit positionInfoUpdated(update); |
||||||
|
} |
||||||
|
|
||||||
|
int QGCPositionManager::updateInterval() const |
||||||
|
{ |
||||||
|
return _updateInterval; |
||||||
|
} |
||||||
|
|
||||||
|
void QGCPositionManager::setPositionSource(QGCPositionManager::QGCPositionSource source) |
||||||
|
{ |
||||||
|
if(_currentSource != nullptr) { |
||||||
|
_currentSource->stopUpdates(); |
||||||
|
disconnect(_currentSource, SIGNAL(positionUpdated(QGeoPositionInfo)), this, SLOT(positionUpdated(QGeoPositionInfo))); |
||||||
|
} |
||||||
|
|
||||||
|
switch(source) { |
||||||
|
case QGCPositionManager::Log: |
||||||
|
break; |
||||||
|
case QGCPositionManager::Simulated: |
||||||
|
_currentSource = _simulatedSource; |
||||||
|
break; |
||||||
|
case QGCPositionManager::GPS: |
||||||
|
default:
|
||||||
|
_currentSource = _defaultSource; |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
_updateInterval = _currentSource->minimumUpdateInterval(); |
||||||
|
_currentSource->setPreferredPositioningMethods(QGeoPositionInfoSource::SatellitePositioningMethods); |
||||||
|
_currentSource->setUpdateInterval(_updateInterval); |
||||||
|
_currentSource->startUpdates(); |
||||||
|
|
||||||
|
connect(_currentSource, SIGNAL(positionUpdated(QGeoPositionInfo)), this, SLOT(positionUpdated(QGeoPositionInfo))); |
||||||
|
} |
||||||
|
|
||||||
@ -0,0 +1,62 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <QtPositioning/qgeopositioninfosource.h> |
||||||
|
|
||||||
|
#include <QVariant> |
||||||
|
|
||||||
|
#include "QGCToolbox.h" |
||||||
|
#include "SimulatedPosition.h" |
||||||
|
|
||||||
|
class QGCPositionManager : public QGCTool { |
||||||
|
Q_OBJECT |
||||||
|
|
||||||
|
public: |
||||||
|
|
||||||
|
QGCPositionManager(QGCApplication* app); |
||||||
|
~QGCPositionManager(); |
||||||
|
|
||||||
|
enum QGCPositionSource { |
||||||
|
Simulated, |
||||||
|
GPS, |
||||||
|
Log |
||||||
|
}; |
||||||
|
|
||||||
|
void setPositionSource(QGCPositionSource source); |
||||||
|
|
||||||
|
int updateInterval() const; |
||||||
|
|
||||||
|
private slots: |
||||||
|
void positionUpdated(const QGeoPositionInfo &update); |
||||||
|
|
||||||
|
signals: |
||||||
|
void lastPositionUpdated(bool valid, QVariant lastPosition); |
||||||
|
void positionInfoUpdated(QGeoPositionInfo update); |
||||||
|
|
||||||
|
private: |
||||||
|
int _updateInterval; |
||||||
|
QGeoPositionInfoSource * _currentSource; |
||||||
|
QGeoPositionInfoSource * _defaultSource; |
||||||
|
QGeoPositionInfoSource * _simulatedSource; |
||||||
|
}; |
||||||
@ -0,0 +1,136 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
#include <QtCore> |
||||||
|
#include <QDateTime> |
||||||
|
#include <QDate> |
||||||
|
|
||||||
|
#include "SimulatedPosition.h" |
||||||
|
|
||||||
|
SimulatedPosition::simulated_motion_s SimulatedPosition::_simulated_motion[5] = {{0,250},{0,0},{0, -250},{-250, 0},{0,0}}; |
||||||
|
|
||||||
|
SimulatedPosition::SimulatedPosition() |
||||||
|
: QGeoPositionInfoSource(NULL), |
||||||
|
lat_int(47.3977420*1e7), |
||||||
|
lon_int(8.5455941*1e7), |
||||||
|
_step_cnt(0), |
||||||
|
_simulate_motion_index(0), |
||||||
|
_simulate_motion(true), |
||||||
|
_rotation(0.0F) |
||||||
|
{ |
||||||
|
QDateTime currentDateTime = QDateTime::currentDateTime(); |
||||||
|
|
||||||
|
qsrand(currentDateTime.toTime_t()); |
||||||
|
|
||||||
|
connect(&update_timer, &QTimer::timeout, this, &SimulatedPosition::updatePosition); |
||||||
|
} |
||||||
|
|
||||||
|
QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const |
||||||
|
{ |
||||||
|
return lastPosition; |
||||||
|
} |
||||||
|
|
||||||
|
SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const |
||||||
|
{ |
||||||
|
return AllPositioningMethods; |
||||||
|
} |
||||||
|
|
||||||
|
int SimulatedPosition::minimumUpdateInterval() const |
||||||
|
{ |
||||||
|
return 1000; |
||||||
|
} |
||||||
|
|
||||||
|
void SimulatedPosition::startUpdates() |
||||||
|
{
|
||||||
|
int interval = updateInterval(); |
||||||
|
if (interval < minimumUpdateInterval()) |
||||||
|
interval = minimumUpdateInterval(); |
||||||
|
|
||||||
|
update_timer.setSingleShot(false); |
||||||
|
update_timer.start(interval); |
||||||
|
} |
||||||
|
|
||||||
|
void SimulatedPosition::stopUpdates() |
||||||
|
{ |
||||||
|
update_timer.stop(); |
||||||
|
} |
||||||
|
|
||||||
|
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() |
||||||
|
{ |
||||||
|
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); |
||||||
|
|
||||||
|
if(lat_mov || lon_mov) { |
||||||
|
info.setAttribute(QGeoPositionInfo::Attribute::Direction, 3.14/2); |
||||||
|
info.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, 5); |
||||||
|
} |
||||||
|
|
||||||
|
lastPosition = info; |
||||||
|
|
||||||
|
emit positionUpdated(info); |
||||||
|
} |
||||||
|
|
||||||
|
QGeoPositionInfoSource::Error SimulatedPosition::error() const |
||||||
|
{ |
||||||
|
return QGeoPositionInfoSource::NoError; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,75 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <QtPositioning/qgeopositioninfosource.h> |
||||||
|
#include "QGCToolbox.h" |
||||||
|
#include <QTimer> |
||||||
|
|
||||||
|
class SimulatedPosition : public QGeoPositionInfoSource |
||||||
|
{ |
||||||
|
Q_OBJECT |
||||||
|
|
||||||
|
public: |
||||||
|
SimulatedPosition(); |
||||||
|
|
||||||
|
QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const; |
||||||
|
|
||||||
|
PositioningMethods supportedPositioningMethods() const; |
||||||
|
int minimumUpdateInterval() const; |
||||||
|
Error error() const; |
||||||
|
|
||||||
|
public slots: |
||||||
|
virtual void startUpdates(); |
||||||
|
virtual void stopUpdates(); |
||||||
|
|
||||||
|
virtual void requestUpdate(int timeout = 5000); |
||||||
|
|
||||||
|
private slots: |
||||||
|
void updatePosition(); |
||||||
|
|
||||||
|
private: |
||||||
|
QTimer update_timer; |
||||||
|
|
||||||
|
QGeoPositionInfo lastPosition; |
||||||
|
|
||||||
|
// items for simulating QGC movment 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; |
||||||
|
|
||||||
|
bool _simulate_motion; |
||||||
|
float _rotation; |
||||||
|
}; |
||||||
Loading…
Reference in new issue