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 14
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QList>
#include <QApplication>
#include <QMessageBox>
#include <QTimer>
15
#include <QSettings>
pixhawk's avatar
pixhawk committed
16
#include "UAS.h"
17 18 19
#include "UASInterface.h"
#include "UASManager.h"
#include "QGC.h"
pixhawk's avatar
pixhawk committed
20

LM's avatar
LM committed
21 22 23 24
#define PI 3.1415926535897932384626433832795
#define MEAN_EARTH_DIAMETER	12756274.0
#define UMR	0.017453292519943295769236907684886

25 26
UASManager* UASManager::instance()
{
pixhawk's avatar
pixhawk committed
27 28 29 30
    static UASManager* _instance = 0;
    if(_instance == 0) {
        _instance = new UASManager();

pixhawk's avatar
pixhawk committed
31 32
        // Set the application as parent to ensure that this object
        // will be destroyed when the main application exits
pixhawk's avatar
pixhawk committed
33 34 35 36 37
        _instance->setParent(qApp);
    }
    return _instance;
}

38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
void UASManager::storeSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_UASMANAGER");
    settings.setValue("HOMELAT", homeLat);
    settings.setValue("HOMELON", homeLon);
    settings.setValue("HOMEALT", homeAlt);
    settings.endGroup();
    settings.sync();
}

void UASManager::loadSettings()
{
    QSettings settings;
    settings.sync();
    settings.beginGroup("QGC_UASMANAGER");
54
    bool changed =  setHomePosition(settings.value("HOMELAT", homeLat).toDouble(),
55 56
                                    settings.value("HOMELON", homeLon).toDouble(),
                                    settings.value("HOMEALT", homeAlt).toDouble());
57 58 59 60 61 62 63 64

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

65 66 67
    settings.endGroup();
}

68
bool UASManager::setHomePosition(double lat, double lon, double alt)
69 70
{
    // Checking for NaN and infitiny
71 72 73 74 75 76
    // 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)
        {
77

78 79 80 81
        if (homeLat != lat) changed = true;
        if (homeLon != lon) changed = true;
        if (homeAlt != alt) changed = true;

82 83 84
        // Initialize conversion reference in any case
        initReference(lat, lon, alt);

85 86 87 88 89
        if (changed)
        {
            homeLat = lat;
            homeLon = lon;
            homeAlt = alt;
90

91 92 93 94 95 96 97 98 99 100 101
            emit homePositionChanged(homeLat, homeLon, homeAlt);

            // Update all UAVs
            foreach (UASInterface* mav, systems)
            {
                mav->setHomePosition(homeLat, homeLon, homeAlt);
            }
        }
    }
    return changed;
}
102

103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122
/**
 * @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
123

124 125 126 127 128 129
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
130

131 132 133
    R(0, 0) = -s_long;
    R(0, 1) = c_long;
    R(0, 2) = 0;
LM's avatar
LM committed
134

135 136 137
    R(1, 0) = -s_lat * c_long;
    R(1, 1) = -s_lat * s_long;
    R(1, 2) = c_lat;
LM's avatar
LM committed
138

139 140 141
    R(2, 0) = c_lat * c_long;
    R(2, 1) = c_lat * s_long;
    R(2, 2) = s_lat;
LM's avatar
LM committed
142

143
    ecef_ref_orientation_ = Eigen::Quaterniond(R);
LM's avatar
LM committed
144

145 146
    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
}
LM's avatar
LM committed
147

148 149 150 151
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
152

153 154 155
    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
156

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

159
    Eigen::Vector3d ecef;
LM's avatar
LM committed
160

161 162 163
    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
164

165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180
    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
181

182
//void UASManager::wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down)
LM's avatar
LM committed
183
//{
184

LM's avatar
LM committed
185 186 187
//}


LM's avatar
LM committed
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203

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


204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224
/**
 * 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)
{
    // FIXME: Accept any home position change for now from the active UAS
    // this means that the currently select UAS can change the home location
    // of the whole swarm. This makes sense, but more control might be needed
    if (uav == activeUAS->getUASID())
    {
        if (setHomePosition(lat, lon, alt))
        {
            foreach (UASInterface* mav, systems)
            {
                // Only update the other systems, not the original source
                if (mav->getUASID() != uav)
                {
                    mav->setHomePosition(homeLat, homeLon, homeAlt);
                }
            }
        }
225 226 227
    }
}

pixhawk's avatar
pixhawk committed
228 229 230 231 232 233
/**
 * @brief Private singleton constructor
 *
 * This class implements the singleton design pattern and has therefore only a private constructor.
 **/
UASManager::UASManager() :
234 235 236
        activeUAS(NULL),
        homeLat(47.3769),
        homeLon(8.549444),
237
        homeAlt(470.0),
238 239
        homeFrame(MAV_FRAME_GLOBAL),
        offlineUASWaypointManager(NULL)
pixhawk's avatar
pixhawk committed
240
{
241
    loadSettings();
242
    setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
pixhawk's avatar
pixhawk committed
243 244 245 246
}

UASManager::~UASManager()
{
247
    storeSettings();
248
    // Delete all systems
249
    foreach (UASInterface* mav, systems) {
250 251
        delete mav;
    }
pixhawk's avatar
pixhawk committed
252 253 254 255
}

void UASManager::addUAS(UASInterface* uas)
{
256 257 258 259 260 261
    // 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
262 263
    if (activeUAS == NULL)
    {
264 265 266 267
        firstUAS = true;
        activeUAS = uas;
    }

pixhawk's avatar
pixhawk committed
268
    // Only execute if there is no UAS at this index
LM's avatar
LM committed
269 270
    if (!systems.contains(uas))
    {
271
        systems.append(uas);
272 273 274 275
        // 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
276 277 278 279
        emit UASCreated(uas);
    }

    // If there is no active UAS yet, set the first one as the active UAS
LM's avatar
LM committed
280 281
    if (firstUAS)
    {
282
        setActiveUAS(uas);
283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298
        if (offlineUASWaypointManager->getWaypointEditableList().size() > 0)
        {
            if (QMessageBox::question(0,"Question","Do you want to append the offline waypoints to the ones currently on the UAV?",QMessageBox::Yes,QMessageBox::No) == QMessageBox::Yes)
            {
                //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
299 300 301
    }
}

302 303 304 305 306 307
/**
 * @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)
308
{
309 310 311
    if (uas)
    {
        int listindex = systems.indexOf(uas);
312

313 314 315 316 317
        // 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.
318
        if (uas == activeUAS)
319
        {
320
            if (systems.count())
321
            {
322
                setActiveUAS(systems.first());
323 324 325
            }
            else
            {
326
                setActiveUAS(NULL);
327 328
            }
        }
329 330 331 332 333

        // 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();
334 335 336
    }
}

lm's avatar
lm committed
337 338
QList<UASInterface*> UASManager::getUASList()
{
339
    return systems;
lm's avatar
lm committed
340 341
}

pixhawk's avatar
pixhawk committed
342 343 344 345 346
UASInterface* UASManager::getActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}

347 348 349 350
UASInterface* UASManager::silentGetActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}
351 352 353 354 355 356 357 358 359 360 361 362 363 364
UASWaypointManager *UASManager::getActiveUASWaypointManager()
{
    if (activeUAS)
    {
        return activeUAS->getWaypointManager();
    }
    if (!offlineUASWaypointManager)
    {
        offlineUASWaypointManager = new UASWaypointManager(NULL);
    }
    return offlineUASWaypointManager;


}
365

pixhawk's avatar
pixhawk committed
366 367 368 369 370 371 372 373 374 375 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
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);
}

void UASManager::configureActiveUAS()
{
    UASInterface* actUAS = getActiveUAS();
416
    if(actUAS) {
pixhawk's avatar
pixhawk committed
417 418 419 420 421 422
        // Do something
    }
}

UASInterface* UASManager::getUASForId(int id)
{
423 424
    UASInterface* system = NULL;

425 426
    foreach(UASInterface* sys, systems) {
        if (sys->getUASID() == id) {
427 428 429 430 431 432
            system = sys;
        }
    }

    // Return NULL if not found
    return system;
pixhawk's avatar
pixhawk committed
433 434
}

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

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