HomePositionManager.cc 7.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2015 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/>.
 
 ======================================================================*/
pixhawk's avatar
pixhawk committed
23 24 25 26

#include <QList>
#include <QApplication>
#include <QTimer>
27
#include <QSettings>
28
#include <QtQml>
Don Gagne's avatar
Don Gagne committed
29

pixhawk's avatar
pixhawk committed
30
#include "UAS.h"
31
#include "UASInterface.h"
32
#include "HomePositionManager.h"
33
#include "QGC.h"
Don Gagne's avatar
Don Gagne committed
34
#include "QGCMessageBox.h"
35
#include "QGCApplication.h"
36
#include "MultiVehicleManager.h"
pixhawk's avatar
pixhawk committed
37

LM's avatar
LM committed
38 39 40 41
#define PI 3.1415926535897932384626433832795
#define MEAN_EARTH_DIAMETER	12756274.0
#define UMR	0.017453292519943295769236907684886

42
IMPLEMENT_QGC_SINGLETON(HomePositionManager, HomePositionManager)
43

44 45 46 47 48 49 50 51 52 53
const char* HomePositionManager::_settingsGroup =   "HomePositionManager";
const char* HomePositionManager::_latitudeKey =     "Latitude";
const char* HomePositionManager::_longitudeKey =    "Longitude";
const char* HomePositionManager::_altitudeKey =     "Altitude";

HomePositionManager::HomePositionManager(QObject* parent)
    : QObject(parent)
    , homeLat(47.3769)
    , homeLon(8.549444)
    , homeAlt(470.0)
54
{
55 56
    qmlRegisterUncreatableType<HomePositionManager> ("QGroundControl", 1, 0, "HomePositionManager", "Reference only");
    
57
    _loadSettings();
58 59
}

60
HomePositionManager::~HomePositionManager()
61
{
62

63 64
}

65
void HomePositionManager::_storeSettings(void)
66 67
{
    QSettings settings;
68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86
    
    settings.remove(_settingsGroup);
    settings.beginGroup(_settingsGroup);
    
    for (int i=0; i<_homePositions.count(); i++) {
        HomePosition* homePos = qobject_cast<HomePosition*>(_homePositions[i]);
        
        qDebug() << "Saving" << homePos->name();
        
        settings.beginGroup(homePos->name());
        settings.setValue(_latitudeKey, homePos->coordinate().latitude());
        settings.setValue(_longitudeKey, homePos->coordinate().longitude());
        settings.setValue(_altitudeKey, homePos->coordinate().altitude());
        settings.endGroup();
    }
    
    settings.endGroup();
    
    // Deprecated settings for old editor
87 88 89 90 91 92 93
    settings.beginGroup("QGC_UASMANAGER");
    settings.setValue("HOMELAT", homeLat);
    settings.setValue("HOMELON", homeLon);
    settings.setValue("HOMEALT", homeAlt);
    settings.endGroup();
}

94
void HomePositionManager::_loadSettings(void)
95 96
{
    QSettings settings;
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
    
    _homePositions.clear();
    
    settings.beginGroup(_settingsGroup);
    
    foreach(QString name, settings.childGroups()) {
        QGeoCoordinate coordinate;
        
        qDebug() << "Load setting" << name;
        
        settings.beginGroup(name);
        coordinate.setLatitude(settings.value(_latitudeKey).toDouble());
        coordinate.setLongitude(settings.value(_longitudeKey).toDouble());
        coordinate.setAltitude(settings.value(_altitudeKey).toDouble());
        settings.endGroup();
        
        _homePositions.append(new HomePosition(name, coordinate, this));
    }
    
    settings.endGroup();
    
    if (_homePositions.count() == 0) {
        _homePositions.append(new HomePosition("ETH Campus", QGeoCoordinate(47.3769, 8.549444, 470.0)));
    }
    
    // Deprecated settings for old editor

124
    settings.beginGroup("QGC_UASMANAGER");
125
    bool changed =  setHomePosition(settings.value("HOMELAT", homeLat).toDouble(),
126 127
                                    settings.value("HOMELON", homeLon).toDouble(),
                                    settings.value("HOMEALT", homeAlt).toDouble());
128 129 130 131 132 133 134 135

    // Make sure to fire the change - this will
    // make sure widgets get the signal once
    if (!changed)
    {
        emit homePositionChanged(homeLat, homeLon, homeAlt);
    }

136 137 138
    settings.endGroup();
}

139
bool HomePositionManager::setHomePosition(double lat, double lon, double alt)
140 141
{
    // Checking for NaN and infitiny
142 143 144 145 146 147
    // and checking for borders
    bool changed = false;
    if (!isnan(lat) && !isnan(lon) && !isnan(alt)
        && !isinf(lat) && !isinf(lon) && !isinf(alt)
        && lat <= 90.0 && lat >= -90.0 && lon <= 180.0 && lon >= -180.0)
        {
148

149 150 151
        if (fabs(homeLat - lat) > 1e-7) changed = true;
        if (fabs(homeLon - lon) > 1e-7) changed = true;
        if (fabs(homeAlt - alt) > 0.5f) changed = true;
152

153 154 155 156 157
        if (changed)
        {
            homeLat = lat;
            homeLon = lon;
            homeAlt = alt;
158

159 160 161 162 163
            emit homePositionChanged(homeLat, homeLon, homeAlt);
        }
    }
    return changed;
}
164

165
bool HomePositionManager::setHomePositionAndNotify(double lat, double lon, double alt)
166 167 168 169 170
{
    // Checking for NaN and infitiny
    // and checking for borders
    bool changed = setHomePosition(lat, lon, alt);

171 172
    if (changed) {
        MultiVehicleManager::instance()->setHomePositionForAllVehicles(homeLat, homeLon, homeAlt);
173
    }
Lorenz Meier's avatar
Lorenz Meier committed
174 175

	return changed;
176 177
}

178
void HomePositionManager::updateHomePosition(const QString& name, const QGeoCoordinate& coordinate)
179
{
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198
    HomePosition * homePos = NULL;
    
    for (int i=0; i<_homePositions.count(); i++) {
        homePos = qobject_cast<HomePosition*>(_homePositions[i]);
        if (homePos->name() == name) {
            break;
        }
        homePos = NULL;
    }
    
    if (homePos == NULL) {
        HomePosition* homePos = new HomePosition(name, coordinate, this);
        _homePositions.append(homePos);
    } else {
        homePos->setName(name);
        homePos->setCoordinate(coordinate);
    }
    
    _storeSettings();
199
}
LM's avatar
LM committed
200

201
void HomePositionManager::deleteHomePosition(const QString& name)
202
{
203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
    // Don't allow delete of last position
    if (_homePositions.count() == 1) {
        return;
    }
    
    qDebug() << "Attempting delete" << name;
    
    for (int i=0; i<_homePositions.count(); i++) {
        if (qobject_cast<HomePosition*>(_homePositions[i])->name() == name) {
            qDebug() << "Deleting" << name;
            _homePositions.removeAt(i);
            break;
        }
    }
    
    _storeSettings();
}
LM's avatar
LM committed
220

221 222 223 224 225
HomePosition::HomePosition(const QString& name, const QGeoCoordinate& coordinate, QObject* parent)
    : QObject(parent)
    , _coordinate(coordinate)
{
    setObjectName(name);
226 227
}

228
HomePosition::~HomePosition()
229
{
230
    
231 232
}

233
QString HomePosition::name(void)
234
{
235
    return objectName();
236
}
LM's avatar
LM committed
237

238
void HomePosition::setName(const QString& name)
LM's avatar
LM committed
239
{
240 241 242
    setObjectName(name);
    HomePositionManager::instance()->_storeSettings();
    emit nameChanged(name);
LM's avatar
LM committed
243 244
}

245
QGeoCoordinate HomePosition::coordinate(void)
LM's avatar
LM committed
246
{
247
    return _coordinate;
LM's avatar
LM committed
248 249
}

250 251 252 253 254 255
void HomePosition::setCoordinate(const QGeoCoordinate& coordinate)
{
    _coordinate = coordinate;
    HomePositionManager::instance()->_storeSettings();
    emit coordinateChanged(coordinate);
}