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>
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"
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
    }
59 60
}

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);
    }

86 87 88
    settings.endGroup();
}

89
bool UASManager::setHomePosition(double lat, double lon, double alt)
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;
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;
}
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
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
LM committed
163

164 165 166
    R(0, 0) = -s_long;
    R(0, 1) = c_long;
    R(0, 2) = 0;
LM's avatar
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
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
LM committed
175

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

178 179
    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
}
LM's avatar
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
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
LM committed
189

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

192
    Eigen::Vector3d ecef;
LM's avatar
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
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
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
LM committed
216
//{
217

LM's avatar
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
        }
259 260 261
    }
}

pixhawk's avatar
pixhawk committed
262 263
void UASManager::addUAS(UASInterface* uas)
{
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)
    {
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();
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

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

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

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

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


}
376

pixhawk's avatar
pixhawk committed
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 425
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)
{
426 427
    UASInterface* system = NULL;

428 429
    foreach(UASInterface* sys, systems) {
        if (sys->getUASID() == id) {
430 431 432 433 434 435
            system = sys;
        }
    }

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

pixhawk's avatar
pixhawk committed
438
void UASManager::setActiveUAS(UASInterface* uas)
pixhawk's avatar
pixhawk committed
439
{
440 441 442 443 444 445 446 447
    // 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
448

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