UASManager.cc 11 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

21 22
UASManager* UASManager::instance()
{
pixhawk's avatar
pixhawk committed
23 24 25 26
    static UASManager* _instance = 0;
    if(_instance == 0) {
        _instance = new UASManager();

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

34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
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");
50
    bool changed =  setHomePosition(settings.value("HOMELAT", homeLat).toDouble(),
51 52
                                    settings.value("HOMELON", homeLon).toDouble(),
                                    settings.value("HOMEALT", homeAlt).toDouble());
53 54 55 56 57 58 59 60

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

61 62 63
    settings.endGroup();
}

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

74 75 76 77
        if (homeLat != lat) changed = true;
        if (homeLon != lon) changed = true;
        if (homeAlt != alt) changed = true;

78 79 80
        // Initialize conversion reference in any case
        initReference(lat, lon, alt);

81 82 83 84 85
        if (changed)
        {
            homeLat = lat;
            homeLon = lon;
            homeAlt = alt;
86

87 88 89 90 91 92 93 94 95 96 97
            emit homePositionChanged(homeLat, homeLon, homeAlt);

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

99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
/**
 * @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
119

120 121 122 123 124 125
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
126

127 128 129
    R(0, 0) = -s_long;
    R(0, 1) = c_long;
    R(0, 2) = 0;
LM's avatar
LM committed
130

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

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

139
    ecef_ref_orientation_ = Eigen::Quaterniond(R);
LM's avatar
LM committed
140

141 142
    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
}
LM's avatar
LM committed
143

144 145 146 147
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
148

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

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

155
    Eigen::Vector3d ecef;
LM's avatar
LM committed
156

157 158 159
    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
160

161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176
    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
177

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

LM's avatar
LM committed
181 182 183
//}


184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
/**
 * 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);
                }
            }
        }
205 206 207
    }
}

pixhawk's avatar
pixhawk committed
208 209 210 211 212 213
/**
 * @brief Private singleton constructor
 *
 * This class implements the singleton design pattern and has therefore only a private constructor.
 **/
UASManager::UASManager() :
214 215 216 217
        activeUAS(NULL),
        homeLat(47.3769),
        homeLon(8.549444),
        homeAlt(470.0)
pixhawk's avatar
pixhawk committed
218 219
{
    start(QThread::LowPriority);
220
    loadSettings();
221
    setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
pixhawk's avatar
pixhawk committed
222 223 224 225
}

UASManager::~UASManager()
{
226
    storeSettings();
227
    // Delete all systems
228
    foreach (UASInterface* mav, systems) {
229 230
        delete mav;
    }
pixhawk's avatar
pixhawk committed
231 232 233 234 235
}


void UASManager::run()
{
236 237 238 239
    //    forever
    //    {
    //        QGC::SLEEP::msleep(5000);
    //    }
lm's avatar
lm committed
240
    exec();
pixhawk's avatar
pixhawk committed
241 242 243 244
}

void UASManager::addUAS(UASInterface* uas)
{
245 246 247 248 249 250
    // 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;
251
    if (activeUAS == NULL) {
252 253 254 255
        firstUAS = true;
        activeUAS = uas;
    }

pixhawk's avatar
pixhawk committed
256
    // Only execute if there is no UAS at this index
257
    if (!systems.contains(uas)) {
258
        systems.append(uas);
259
        connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(removeUAS(QObject*)));
260 261 262 263
        // 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
264 265 266 267
        emit UASCreated(uas);
    }

    // If there is no active UAS yet, set the first one as the active UAS
268
    if (firstUAS) {
269
        setActiveUAS(uas);
pixhawk's avatar
pixhawk committed
270 271 272
    }
}

273 274 275 276
void UASManager::removeUAS(QObject* uas)
{
    UASInterface* mav = qobject_cast<UASInterface*>(uas);

277
    if (mav) {
278 279
        int listindex = systems.indexOf(mav);

280 281
        if (mav == activeUAS) {
            if (systems.count() > 1) {
282
                // We only set a new UAS if more than one is present
283
                if (listindex != 0) {
284 285 286
                    // The system to be removed is not at position 1
                    // set position one as new active system
                    setActiveUAS(systems.first());
287
                } else {
288 289 290 291
                    // The system to be removed is at position 1,
                    // select the next system
                    setActiveUAS(systems.at(1));
                }
292
            } else {
293 294 295 296 297 298 299 300 301
                // TODO send a null pointer if no UAS is present any more
                // This has to be proberly tested however, since it might
                // crash code parts not handling null pointers correctly.
            }
        }
        systems.removeAt(listindex);
    }
}

lm's avatar
lm committed
302 303
QList<UASInterface*> UASManager::getUASList()
{
304
    return systems;
lm's avatar
lm committed
305 306
}

pixhawk's avatar
pixhawk committed
307 308 309 310 311
UASInterface* UASManager::getActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}

312 313 314 315 316
UASInterface* UASManager::silentGetActiveUAS()
{
    return activeUAS; ///< Return zero pointer if no UAS has been loaded
}

pixhawk's avatar
pixhawk committed
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366
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();
367
    if(actUAS) {
pixhawk's avatar
pixhawk committed
368 369 370 371 372 373
        // Do something
    }
}

UASInterface* UASManager::getUASForId(int id)
{
374 375
    UASInterface* system = NULL;

376 377
    foreach(UASInterface* sys, systems) {
        if (sys->getUASID() == id) {
378 379 380 381 382 383
            system = sys;
        }
    }

    // Return NULL if not found
    return system;
pixhawk's avatar
pixhawk committed
384 385
}

pixhawk's avatar
pixhawk committed
386
void UASManager::setActiveUAS(UASInterface* uas)
pixhawk's avatar
pixhawk committed
387
{
388
    if (uas != NULL) {
pixhawk's avatar
pixhawk committed
389
        activeUASMutex.lock();
390
        if (activeUAS != NULL) {
391 392 393
            emit activeUASStatusChanged(activeUAS, false);
            emit activeUASStatusChanged(activeUAS->getUASID(), false);
        }
pixhawk's avatar
pixhawk committed
394 395
        activeUAS = uas;
        activeUASMutex.unlock();
pixhawk's avatar
pixhawk committed
396

397
        activeUAS->setSelected();
pixhawk's avatar
pixhawk committed
398
        emit activeUASSet(uas);
399
        emit activeUASSet(uas->getUASID());
400
        emit activeUASSetListIndex(systems.indexOf(uas));
401 402
        emit activeUASStatusChanged(uas, true);
        emit activeUASStatusChanged(uas->getUASID(), true);
pixhawk's avatar
pixhawk committed
403
    }
pixhawk's avatar
pixhawk committed
404 405
}