Lorenz Meier
9 years ago
14 changed files with 441 additions and 130 deletions
@ -0,0 +1,88 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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