UASManager.cc 12.9 KB
Newer Older
1
/*==================================================================
pixhawk's avatar
pixhawk committed
2
3
4
5
======================================================================*/

/**
 * @file
6
 *   @brief Implementation of class UASManager
pixhawk's avatar
pixhawk committed
7
8
9
10
11
12
13
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QList>
#include <QApplication>
#include <QTimer>
lm's avatar
lm committed
14
#include <QSettings>
Don Gagne's avatar
Don Gagne committed
15

pixhawk's avatar
pixhawk committed
16
#include "UAS.h"
17
18
19
#include "UASInterface.h"
#include "UASManager.h"
#include "QGC.h"
Don Gagne's avatar
Don Gagne committed
20
#include "QGCMessageBox.h"
Don Gagne's avatar
Don Gagne committed
21
#include "QGCApplication.h"
pixhawk's avatar
pixhawk committed
22

LM's avatar
LM committed
23
24
25
26
#define PI 3.1415926535897932384626433832795
#define MEAN_EARTH_DIAMETER	12756274.0
#define UMR	0.017453292519943295769236907684886

Don Gagne's avatar
Don Gagne committed
27
IMPLEMENT_QGC_SINGLETON(UASManager, UASManagerInterface)
28

Don Gagne's avatar
Don Gagne committed
29
30
31
32
33
34
35
36
UASManager::UASManager(QObject* parent) :
    UASManagerInterface(parent),
    activeUAS(NULL),
    offlineUASWaypointManager(NULL),
    homeLat(47.3769),
    homeLon(8.549444),
    homeAlt(470.0),
    homeFrame(MAV_FRAME_GLOBAL)
37
{
Don Gagne's avatar
Don Gagne committed
38
39
    loadSettings();
    setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
40
41
}

Don Gagne's avatar
Don Gagne committed
42
UASManager::~UASManager()
43
{
Don Gagne's avatar
Don Gagne committed
44
    storeSettings();
45
46
47
48
49
50
51
52
53
54
55
56
57
    Q_ASSERT_X(systems.count() == 0, "~UASManager", "_shutdown should have already removed all uas");
}

void UASManager::_shutdown(void)
{
    QList<UASInterface*> uasList;
    
    foreach(UASInterface* uas, systems) {
        uasList.append(uas);
    }
    
    foreach(UASInterface* uas, uasList) {
        removeUAS(uas);
pixhawk's avatar
pixhawk committed
58
    }
Don Gagne's avatar
Don Gagne committed
59
60
}

lm's avatar
lm committed
61
62
63
64
65
66
67
68
69
70
71
72
73
74
void UASManager::storeSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_UASMANAGER");
    settings.setValue("HOMELAT", homeLat);
    settings.setValue("HOMELON", homeLon);
    settings.setValue("HOMEALT", homeAlt);
    settings.endGroup();
}

void UASManager::loadSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_UASMANAGER");
75
    bool changed =  setHomePosition(settings.value("HOMELAT", homeLat).toDouble(),
76
77
                                    settings.value("HOMELON", homeLon).toDouble(),
                                    settings.value("HOMEALT", homeAlt).toDouble());
78
79
80
81
82
83
84
85

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

lm's avatar
lm committed
86
87
88
    settings.endGroup();
}

89
bool UASManager::setHomePosition(double lat, double lon, double alt)
lm's avatar
lm committed
90
91
{
    // Checking for NaN and infitiny
92
93
94
95
96
97
    // 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)
        {
98

99
100
101
        if (fabs(homeLat - lat) > 1e-7) changed = true;
        if (fabs(homeLon - lon) > 1e-7) changed = true;
        if (fabs(homeAlt - alt) > 0.5f) changed = true;
lm's avatar
lm committed
102

103
104
105
        // Initialize conversion reference in any case
        initReference(lat, lon, alt);

106
107
108
109
110
        if (changed)
        {
            homeLat = lat;
            homeLon = lon;
            homeAlt = alt;
111

112
113
114
115
116
            emit homePositionChanged(homeLat, homeLon, homeAlt);
        }
    }
    return changed;
}
lm's avatar
lm committed
117

118
119
120
121
122
123
124
125
126
127
128
129
130
131
bool UASManager::setHomePositionAndNotify(double lat, double lon, double alt)
{
    // Checking for NaN and infitiny
    // and checking for borders
    bool changed = setHomePosition(lat, lon, alt);

    if (changed)
    {
        // Update all UAVs
        foreach (UASInterface* mav, systems)
        {
            mav->setHomePosition(homeLat, homeLon, homeAlt);
        }
    }
Lorenz Meier's avatar
Lorenz Meier committed
132
133

	return changed;
134
135
}

136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
/**
 * @param x1 Point 1 coordinate in x dimension
 * @param y1 Point 1 coordinate in y dimension
 * @param z1 Point 1 coordinate in z dimension
 *
 * @param x2 Point 2 coordinate in x dimension
 * @param y2 Point 2 coordinate in y dimension
 * @param z2 Point 2 coordinate in z dimension
 */
void UASManager::setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2)
{
    nedSafetyLimitPosition1.x() = x1;
    nedSafetyLimitPosition1.y() = y1;
    nedSafetyLimitPosition1.z() = z1;

    nedSafetyLimitPosition2.x() = x2;
    nedSafetyLimitPosition2.y() = y2;
    nedSafetyLimitPosition2.z() = z2;
}

LM's avatar
Working    
LM committed
156

157
158
159
160
161
162
void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude)
{
    Eigen::Matrix3d R;
    double s_long, s_lat, c_long, c_lat;
    sincos(latitude * DEG2RAD, &s_lat, &c_lat);
    sincos(longitude * DEG2RAD, &s_long, &c_long);
LM's avatar
Working    
LM committed
163

164
165
166
    R(0, 0) = -s_long;
    R(0, 1) = c_long;
    R(0, 2) = 0;
LM's avatar
Working    
LM committed
167

168
169
170
    R(1, 0) = -s_lat * c_long;
    R(1, 1) = -s_lat * s_long;
    R(1, 2) = c_lat;
LM's avatar
Working    
LM committed
171

172
173
174
    R(2, 0) = c_lat * c_long;
    R(2, 1) = c_lat * s_long;
    R(2, 2) = s_lat;
LM's avatar
Working    
LM committed
175

176
    ecef_ref_orientation_ = Eigen::Quaterniond(R);
LM's avatar
Working    
LM committed
177

178
179
    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
}
LM's avatar
Working    
LM committed
180

181
182
183
184
Eigen::Vector3d UASManager::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude)
{
    const double a = 6378137.0; // semi-major axis
    const double e_sq = 6.69437999014e-3; // first eccentricity squared
LM's avatar
Working    
LM committed
185

186
187
188
    double s_long, s_lat, c_long, c_lat;
    sincos(latitude * DEG2RAD, &s_lat, &c_lat);
    sincos(longitude * DEG2RAD, &s_long, &c_long);
LM's avatar
Working    
LM committed
189

190
    const double N = a / sqrt(1 - e_sq * s_lat * s_lat);
LM's avatar
Working    
LM committed
191

192
    Eigen::Vector3d ecef;
LM's avatar
Working    
LM committed
193

194
195
196
    ecef[0] = (N + altitude) * c_lat * c_long;
    ecef[1] = (N + altitude) * c_lat * s_long;
    ecef[2] = (N * (1 - e_sq) + altitude) * s_lat;
LM's avatar
Working    
LM committed
197

198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
    return ecef;
}

Eigen::Vector3d UASManager::ecefToEnu(const Eigen::Vector3d & ecef)
{
    return ecef_ref_orientation_ * (ecef - ecef_ref_point_);
}

void UASManager::wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up)
{
    Eigen::Vector3d ecef = wgs84ToEcef(lat, lon, alt);
    Eigen::Vector3d enu = ecefToEnu(ecef);
    *east = enu.x();
    *north = enu.y();
    *up = enu.z();
}
LM's avatar
Working    
LM committed
214

215
//void UASManager::wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down)
LM's avatar
Working    
LM committed
216
//{
217

LM's avatar
Working    
LM committed
218
219
220
//}


LM's avatar
LM committed
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236

void UASManager::enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
{
    *lat=homeLat+y/MEAN_EARTH_DIAMETER*360./PI;
    *lon=homeLon+x/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
    *alt=homeAlt+z;
}

void UASManager::nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
{
    *lat=homeLat+x/MEAN_EARTH_DIAMETER*360./PI;
    *lon=homeLon+y/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
    *alt=homeAlt-z;
}


237
238
239
240
241
/**
 * This function will change QGC's home position on a number of conditions only
 */
void UASManager::uavChangedHomePosition(int uav, double lat, double lon, double alt)
{
242
    // Accept home position changes from the active UAS
243
244
245
246
    if (uav == activeUAS->getUASID())
    {
        if (setHomePosition(lat, lon, alt))
        {
247
248
249
250
251
252
253
254
255
256
257
            // XXX DO NOT UPDATE THE WHOLE FLEET


//            foreach (UASInterface* mav, systems)
//            {
//                // Only update the other systems, not the original source
//                if (mav->getUASID() != uav)
//                {
//                    mav->setHomePosition(homeLat, homeLon, homeAlt);
//                }
//            }
258
        }
lm's avatar
lm committed
259
260
261
    }
}

pixhawk's avatar
pixhawk committed
262
263
void UASManager::addUAS(UASInterface* uas)
{
lm's avatar
lm committed
264
265
266
267
268
269
    // WARNING: The active uas is set here
    // and then announced below. This is necessary
    // to make sure the getActiveUAS() function
    // returns the UAS once the UASCreated() signal
    // is emitted. The code is thus NOT redundant.
    bool firstUAS = false;
LM's avatar
LM committed
270
271
    if (activeUAS == NULL)
    {
lm's avatar
lm committed
272
273
274
275
        firstUAS = true;
        activeUAS = uas;
    }

pixhawk's avatar
pixhawk committed
276
    // Only execute if there is no UAS at this index
LM's avatar
LM committed
277
278
    if (!systems.contains(uas))
    {
tstellanova's avatar
tstellanova committed
279
        qDebug() << "Add new UAS: " << uas->getUASID();
lm's avatar
lm committed
280
        systems.append(uas);
281
282
283
284
        // Set home position on UAV if set in UI
        // - this is done on a per-UAV basis
        // Set home position in UI if UAV chooses a new one (caution! if multiple UAVs are connected, take care!)
        connect(uas, SIGNAL(homePositionChanged(int,double,double,double)), this, SLOT(uavChangedHomePosition(int,double,double,double)));
pixhawk's avatar
pixhawk committed
285
286
287
288
        emit UASCreated(uas);
    }

    // If there is no active UAS yet, set the first one as the active UAS
LM's avatar
LM committed
289
290
    if (firstUAS)
    {
291
        setActiveUAS(uas);
292
293
        // Call getActiveUASWaypointManager instead of referencing variable to make sure of creation
        if (getActiveUASWaypointManager()->getWaypointEditableList().size() > 0)
294
        {
Don Gagne's avatar
Don Gagne committed
295
            if (QGCMessageBox::question(tr("Question"), tr("Do you want to append the offline waypoints to the ones currently on the UAV?"), QMessageBox::Yes, QMessageBox::No) == QMessageBox::Yes)
296
297
298
299
300
301
302
303
304
305
306
307
308
            {
                //Need to transfer all waypoints from the offline mode WPManager to the online mode.
                for (int i=0;i<offlineUASWaypointManager->getWaypointEditableList().size();i++)
                {
                    Waypoint *wp = uas->getWaypointManager()->createWaypoint();
                    wp->setLatitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getLatitude());
                    wp->setLongitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getLongitude());
                    wp->setAltitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getAltitude());
                }
            }
            offlineUASWaypointManager->deleteLater();
            offlineUASWaypointManager = 0;
        }
pixhawk's avatar
pixhawk committed
309
310
311
    }
}

312
313
314
315
316
317
/**
 * @brief The function that should be used when removing UASes from QGC. emits UASDeletect(UASInterface*) when triggered
 *        so that UI elements can update accordingly.
 * @param uas The UAS to remove
 */
void UASManager::removeUAS(UASInterface* uas)
318
{
319
320
321
    if (uas)
    {
        int listindex = systems.indexOf(uas);
322

323
324
325
326
327
        // Remove this system from local data store.
        systems.removeAt(listindex);

        // If this is the active UAS, select a new one if one exists otherwise
        // indicate that there are no active UASes.
328
        if (uas == activeUAS)
329
        {
330
            if (systems.count())
331
            {
332
                setActiveUAS(systems.first());
333
334
335
            }
            else
            {
336
                setActiveUAS(NULL);
337
338
            }
        }
339
340
341
342
343

        // Notify other UI elements that a UAS is being deleted before finally deleting it.
        qDebug() << "Deleting UAS object: " << uas->getUASName();
        emit UASDeleted(uas);
        uas->deleteLater();
344
345
346
    }
}

lm's avatar
lm committed
347
348
QList<UASInterface*> UASManager::getUASList()
{
lm's avatar
lm committed
349
    return systems;
lm's avatar
lm committed
350
351
}

pixhawk's avatar
pixhawk committed
352
353
354
355
356
UASInterface* UASManager::getActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}

357
358
359
360
UASInterface* UASManager::silentGetActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}
361
362
363
364
365
366
367
368
369
370
371
372
373
374
UASWaypointManager *UASManager::getActiveUASWaypointManager()
{
    if (activeUAS)
    {
        return activeUAS->getWaypointManager();
    }
    if (!offlineUASWaypointManager)
    {
        offlineUASWaypointManager = new UASWaypointManager(NULL);
    }
    return offlineUASWaypointManager;


}
375

pixhawk's avatar
pixhawk committed
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
bool UASManager::launchActiveUAS()
{
    // If the active UAS is set, execute command
    if (getActiveUAS()) activeUAS->launch();
    return (activeUAS); ///< Returns true if the UAS exists, false else
}

bool UASManager::haltActiveUAS()
{
    // If the active UAS is set, execute command
    if (getActiveUAS()) activeUAS->halt();
    return (activeUAS); ///< Returns true if the UAS exists, false else
}

bool UASManager::continueActiveUAS()
{
    // If the active UAS is set, execute command
    if (getActiveUAS()) activeUAS->go();
    return (activeUAS); ///< Returns true if the UAS exists, false else
}

bool UASManager::returnActiveUAS()
{
    // If the active UAS is set, execute command
    if (getActiveUAS()) activeUAS->home();
    return (activeUAS); ///< Returns true if the UAS exists, false else
}

bool UASManager::stopActiveUAS()
{
    // If the active UAS is set, execute command
    if (getActiveUAS()) activeUAS->emergencySTOP();
    return (activeUAS); ///< Returns true if the UAS exists, false else
}

bool UASManager::killActiveUAS()
{
    if (getActiveUAS()) activeUAS->emergencyKILL();
    return (activeUAS);
}

bool UASManager::shutdownActiveUAS()
{
    if (getActiveUAS()) activeUAS->shutdown();
    return (activeUAS);
}

UASInterface* UASManager::getUASForId(int id)
{
lm's avatar
lm committed
425
426
    UASInterface* system = NULL;

427
428
    foreach(UASInterface* sys, systems) {
        if (sys->getUASID() == id) {
lm's avatar
lm committed
429
430
431
432
433
434
            system = sys;
        }
    }

    // Return NULL if not found
    return system;
pixhawk's avatar
pixhawk committed
435
436
}

pixhawk's avatar
pixhawk committed
437
void UASManager::setActiveUAS(UASInterface* uas)
pixhawk's avatar
pixhawk committed
438
{
439
440
441
442
443
444
445
446
    // Signal components that the last UAS is no longer active.
    activeUASMutex.lock();
    if (activeUAS != NULL) {
        emit activeUASStatusChanged(activeUAS, false);
        emit activeUASStatusChanged(activeUAS->getUASID(), false);
    }
    activeUAS = uas;
    activeUASMutex.unlock();
pixhawk's avatar
pixhawk committed
447

448
449
450
451
    // And signal that a new UAS is.
    emit activeUASSet(activeUAS);
    if (activeUAS)
    {
452
        activeUAS->setSelected();
453
454
455
        emit activeUASSetListIndex(systems.indexOf(activeUAS));
        emit activeUASStatusChanged(activeUAS, true);
        emit activeUASStatusChanged(activeUAS->getUASID(), true);
pixhawk's avatar
pixhawk committed
456
    }
pixhawk's avatar
pixhawk committed
457
458
}