UASManager.cc 13.2 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
        if (fabs(homeLat - lat) > 1e-7) changed = true;
        if (fabs(homeLon - lon) > 1e-7) changed = true;
        if (fabs(homeAlt - alt) > 0.5f) changed = true;
81

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
            emit homePositionChanged(homeLat, homeLon, homeAlt);
        }
    }
    return changed;
}
96

97 98 99 100 101 102 103 104 105 106 107 108 109 110
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
111 112

	return changed;
113 114
}

115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134
/**
 * @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
135

136 137 138 139 140 141
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
142

143 144 145
    R(0, 0) = -s_long;
    R(0, 1) = c_long;
    R(0, 2) = 0;
LM's avatar
LM committed
146

147 148 149
    R(1, 0) = -s_lat * c_long;
    R(1, 1) = -s_lat * s_long;
    R(1, 2) = c_lat;
LM's avatar
LM committed
150

151 152 153
    R(2, 0) = c_lat * c_long;
    R(2, 1) = c_lat * s_long;
    R(2, 2) = s_lat;
LM's avatar
LM committed
154

155
    ecef_ref_orientation_ = Eigen::Quaterniond(R);
LM's avatar
LM committed
156

157 158
    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
}
LM's avatar
LM committed
159

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

165 166 167
    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
168

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

171
    Eigen::Vector3d ecef;
LM's avatar
LM committed
172

173 174 175
    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
176

177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192
    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
193

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

LM's avatar
LM committed
197 198 199
//}


LM's avatar
LM committed
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215

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


216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236
/**
 * 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);
                }
            }
        }
237 238 239
    }
}

pixhawk's avatar
pixhawk committed
240 241 242 243 244 245
/**
 * @brief Private singleton constructor
 *
 * This class implements the singleton design pattern and has therefore only a private constructor.
 **/
UASManager::UASManager() :
246
        activeUAS(NULL),
Bill Bonney's avatar
Bill Bonney committed
247
        offlineUASWaypointManager(NULL),
248 249
        homeLat(47.3769),
        homeLon(8.549444),
250
        homeAlt(470.0),
Bill Bonney's avatar
Bill Bonney committed
251
        homeFrame(MAV_FRAME_GLOBAL)
pixhawk's avatar
pixhawk committed
252
{
253
    loadSettings();
254
    setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
pixhawk's avatar
pixhawk committed
255 256 257 258
}

UASManager::~UASManager()
{
259
    storeSettings();
260
    // Delete all systems
261
    foreach (UASInterface* mav, systems) {
262 263
        delete mav;
    }
pixhawk's avatar
pixhawk committed
264 265 266 267
}

void UASManager::addUAS(UASInterface* uas)
{
268 269 270 271 272 273
    // 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
274 275
    if (activeUAS == NULL)
    {
276 277 278 279
        firstUAS = true;
        activeUAS = uas;
    }

pixhawk's avatar
pixhawk committed
280
    // Only execute if there is no UAS at this index
LM's avatar
LM committed
281 282
    if (!systems.contains(uas))
    {
tstellanova's avatar
tstellanova committed
283
        qDebug() << "Add new UAS: " << uas->getUASID();
284
        systems.append(uas);
285 286 287 288
        // 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
289 290 291 292
        emit UASCreated(uas);
    }

    // If there is no active UAS yet, set the first one as the active UAS
LM's avatar
LM committed
293 294
    if (firstUAS)
    {
295
        setActiveUAS(uas);
296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311
        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
312 313 314
    }
}

315 316 317 318 319 320
/**
 * @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)
321
{
322 323 324
    if (uas)
    {
        int listindex = systems.indexOf(uas);
325

326 327 328 329 330
        // 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.
331
        if (uas == activeUAS)
332
        {
333
            if (systems.count())
334
            {
335
                setActiveUAS(systems.first());
336 337 338
            }
            else
            {
339
                setActiveUAS(NULL);
340 341
            }
        }
342 343 344 345 346

        // 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();
347 348 349
    }
}

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

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

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


}
378

pixhawk's avatar
pixhawk committed
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 426 427 428
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();
429
    if(actUAS) {
pixhawk's avatar
pixhawk committed
430 431 432 433 434 435
        // Do something
    }
}

UASInterface* UASManager::getUASForId(int id)
{
436 437
    UASInterface* system = NULL;

438 439
    foreach(UASInterface* sys, systems) {
        if (sys->getUASID() == id) {
440 441 442 443 444 445
            system = sys;
        }
    }

    // Return NULL if not found
    return system;
pixhawk's avatar
pixhawk committed
446 447
}

pixhawk's avatar
pixhawk committed
448
void UASManager::setActiveUAS(UASInterface* uas)
pixhawk's avatar
pixhawk committed
449
{
450 451 452 453 454 455 456 457
    // 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
458

459 460 461 462
    // And signal that a new UAS is.
    emit activeUASSet(activeUAS);
    if (activeUAS)
    {
463
        activeUAS->setSelected();
464 465 466 467
        emit activeUASSet(activeUAS->getUASID());
        emit activeUASSetListIndex(systems.indexOf(activeUAS));
        emit activeUASStatusChanged(activeUAS, true);
        emit activeUASStatusChanged(activeUAS->getUASID(), true);
pixhawk's avatar
pixhawk committed
468
    }
pixhawk's avatar
pixhawk committed
469 470
}