UASManager.cc 13.1 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
        activeUAS(NULL),
Bill Bonney's avatar
Bill Bonney committed
235
        offlineUASWaypointManager(NULL),
236 237
        homeLat(47.3769),
        homeLon(8.549444),
238
        homeAlt(470.0),
Bill Bonney's avatar
Bill Bonney committed
239
        homeFrame(MAV_FRAME_GLOBAL)
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
        connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(removeUAS(QObject*)));
273 274 275 276
        // 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
277 278 279 280
        emit UASCreated(uas);
    }

    // If there is no active UAS yet, set the first one as the active UAS
LM's avatar
LM committed
281 282
    if (firstUAS)
    {
283
        setActiveUAS(uas);
284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299
        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
300 301 302
    }
}

303 304 305 306
void UASManager::removeUAS(QObject* uas)
{
    UASInterface* mav = qobject_cast<UASInterface*>(uas);

307
    if (mav) {
308 309
        int listindex = systems.indexOf(mav);

310 311 312 313
        if (mav == activeUAS)
        {
            if (systems.count() > 1)
            {
314
                // We only set a new UAS if more than one is present
315 316
                if (listindex != 0)
                {
317 318 319
                    // The system to be removed is not at position 1
                    // set position one as new active system
                    setActiveUAS(systems.first());
320 321 322
                }
                else
                {
323 324 325 326
                    // The system to be removed is at position 1,
                    // select the next system
                    setActiveUAS(systems.at(1));
                }
327 328 329
            }
            else
            {
330
                // TODO send a null pointer if no UAS is present any more
331
                // This has to be properly tested however, since it might
332
                // crash code parts not handling null pointers correctly.
333 334
                activeUAS = NULL;
                // XXX Not emitting the null pointer yet
335 336 337
            }
        }
        systems.removeAt(listindex);
338
        emit UASDeleted(mav);
339 340 341
    }
}

lm's avatar
lm committed
342 343
QList<UASInterface*> UASManager::getUASList()
{
344
    return systems;
lm's avatar
lm committed
345 346
}

pixhawk's avatar
pixhawk committed
347 348 349 350 351
UASInterface* UASManager::getActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}

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


}
370

pixhawk's avatar
pixhawk committed
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 416 417 418 419 420
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();
421
    if(actUAS) {
pixhawk's avatar
pixhawk committed
422 423 424 425 426 427
        // Do something
    }
}

UASInterface* UASManager::getUASForId(int id)
{
428 429
    UASInterface* system = NULL;

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

    // Return NULL if not found
    return system;
pixhawk's avatar
pixhawk committed
438 439
}

pixhawk's avatar
pixhawk committed
440
void UASManager::setActiveUAS(UASInterface* uas)
pixhawk's avatar
pixhawk committed
441
{
442
    if (uas != NULL) {
pixhawk's avatar
pixhawk committed
443
        activeUASMutex.lock();
444
        if (activeUAS != NULL) {
445 446 447
            emit activeUASStatusChanged(activeUAS, false);
            emit activeUASStatusChanged(activeUAS->getUASID(), false);
        }
pixhawk's avatar
pixhawk committed
448 449
        activeUAS = uas;
        activeUASMutex.unlock();
pixhawk's avatar
pixhawk committed
450

451
        activeUAS->setSelected();
pixhawk's avatar
pixhawk committed
452
        emit activeUASSet(uas);
453
        emit activeUASSet(uas->getUASID());
454
        emit activeUASSetListIndex(systems.indexOf(uas));
455 456
        emit activeUASStatusChanged(uas, true);
        emit activeUASStatusChanged(uas->getUASID(), true);
pixhawk's avatar
pixhawk committed
457
    }
pixhawk's avatar
pixhawk committed
458 459
}