Add Home Position Manager support to Mission Editor

Also:
- Remove unused code

src/HomePositionManager.cc

@@ -40,24 +40,47 @@
 
 IMPLEMENT_QGC_SINGLETON(HomePositionManager, HomePositionManager)
 
-HomePositionManager::HomePositionManager(QObject* parent) :
-    QObject(parent),
-    homeLat(47.3769),
-    homeLon(8.549444),
-    homeAlt(470.0),
-    homeFrame(MAV_FRAME_GLOBAL)
+const char* HomePositionManager::_settingsGroup =   "HomePositionManager";
+const char* HomePositionManager::_latitudeKey =     "Latitude";
+const char* HomePositionManager::_longitudeKey =    "Longitude";
+const char* HomePositionManager::_altitudeKey =     "Altitude";
+
+HomePositionManager::HomePositionManager(QObject* parent)
+    : QObject(parent)
+    , homeLat(47.3769)
+    , homeLon(8.549444)
+    , homeAlt(470.0)
 {
-    loadSettings();
+    _loadSettings();
 }
 
 HomePositionManager::~HomePositionManager()
 {
-    storeSettings();
+
 }
 
-void HomePositionManager::storeSettings()
+void HomePositionManager::_storeSettings(void)
 {
     QSettings settings;
+    
+    settings.remove(_settingsGroup);
+    settings.beginGroup(_settingsGroup);
+    
+    for (int i=0; i<_homePositions.count(); i++) {
+        HomePosition* homePos = qobject_cast<HomePosition*>(_homePositions[i]);
+        
+        qDebug() << "Saving" << homePos->name();
+        
+        settings.beginGroup(homePos->name());
+        settings.setValue(_latitudeKey, homePos->coordinate().latitude());
+        settings.setValue(_longitudeKey, homePos->coordinate().longitude());
+        settings.setValue(_altitudeKey, homePos->coordinate().altitude());
+        settings.endGroup();
+    }
+    
+    settings.endGroup();
+    
+    // Deprecated settings for old editor
     settings.beginGroup("QGC_UASMANAGER");
     settings.setValue("HOMELAT", homeLat);
     settings.setValue("HOMELON", homeLon);
@@ -65,9 +88,36 @@ void HomePositionManager::storeSettings()
     settings.endGroup();
 }
 
-void HomePositionManager::loadSettings()
+void HomePositionManager::_loadSettings(void)
 {
     QSettings settings;
+    
+    _homePositions.clear();
+    
+    settings.beginGroup(_settingsGroup);
+    
+    foreach(QString name, settings.childGroups()) {
+        QGeoCoordinate coordinate;
+        
+        qDebug() << "Load setting" << name;
+        
+        settings.beginGroup(name);
+        coordinate.setLatitude(settings.value(_latitudeKey).toDouble());
+        coordinate.setLongitude(settings.value(_longitudeKey).toDouble());
+        coordinate.setAltitude(settings.value(_altitudeKey).toDouble());
+        settings.endGroup();
+        
+        _homePositions.append(new HomePosition(name, coordinate, this));
+    }
+    
+    settings.endGroup();
+    
+    if (_homePositions.count() == 0) {
+        _homePositions.append(new HomePosition("ETH Campus", QGeoCoordinate(47.3769, 8.549444, 470.0)));
+    }
+    
+    // Deprecated settings for old editor
+
     settings.beginGroup("QGC_UASMANAGER");
     bool changed =  setHomePosition(settings.value("HOMELAT", homeLat).toDouble(),
                                     settings.value("HOMELON", homeLon).toDouble(),
@@ -97,9 +147,6 @@ bool HomePositionManager::setHomePosition(double lat, double lon, double alt)
         if (fabs(homeLon - lon) > 1e-7) changed = true;
         if (fabs(homeAlt - alt) > 0.5f) changed = true;
 
-        // Initialize conversion reference in any case
-        initReference(lat, lon, alt);
-
         if (changed)
         {
             homeLat = lat;
@@ -125,75 +172,81 @@ bool HomePositionManager::setHomePositionAndNotify(double lat, double lon, doubl
 	return changed;
 }
 
-void HomePositionManager::initReference(const double & latitude, const double & longitude, const double & altitude)
+void HomePositionManager::updateHomePosition(const QString& name, const QGeoCoordinate& coordinate)
 {
-    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);
-
-    R(0, 0) = -s_long;
-    R(0, 1) = c_long;
-    R(0, 2) = 0;
-
-    R(1, 0) = -s_lat * c_long;
-    R(1, 1) = -s_lat * s_long;
-    R(1, 2) = c_lat;
-
-    R(2, 0) = c_lat * c_long;
-    R(2, 1) = c_lat * s_long;
-    R(2, 2) = s_lat;
-
-    ecef_ref_orientation_ = Eigen::Quaterniond(R);
-
-    ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
+    HomePosition * homePos = NULL;
+    
+    for (int i=0; i<_homePositions.count(); i++) {
+        homePos = qobject_cast<HomePosition*>(_homePositions[i]);
+        if (homePos->name() == name) {
+            break;
+        }
+        homePos = NULL;
+    }
+    
+    if (homePos == NULL) {
+        HomePosition* homePos = new HomePosition(name, coordinate, this);
+        _homePositions.append(homePos);
+    } else {
+        homePos->setName(name);
+        homePos->setCoordinate(coordinate);
+    }
+    
+    _storeSettings();
 }
 
-Eigen::Vector3d HomePositionManager::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude)
+void HomePositionManager::deleteHomePosition(const QString& name)
 {
-    const double a = 6378137.0; // semi-major axis
-    const double e_sq = 6.69437999014e-3; // first eccentricity squared
-
-    double s_long, s_lat, c_long, c_lat;
-    sincos(latitude * DEG2RAD, &s_lat, &c_lat);
-    sincos(longitude * DEG2RAD, &s_long, &c_long);
-
-    const double N = a / sqrt(1 - e_sq * s_lat * s_lat);
-
-    Eigen::Vector3d ecef;
-
-    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;
+    // Don't allow delete of last position
+    if (_homePositions.count() == 1) {
+        return;
+    }
+    
+    qDebug() << "Attempting delete" << name;
+    
+    for (int i=0; i<_homePositions.count(); i++) {
+        if (qobject_cast<HomePosition*>(_homePositions[i])->name() == name) {
+            qDebug() << "Deleting" << name;
+            _homePositions.removeAt(i);
+            break;
+        }
+    }
+    
+    _storeSettings();
+}
 
-    return ecef;
+HomePosition::HomePosition(const QString& name, const QGeoCoordinate& coordinate, QObject* parent)
+    : QObject(parent)
+    , _coordinate(coordinate)
+{
+    setObjectName(name);
 }
 
-Eigen::Vector3d HomePositionManager::ecefToEnu(const Eigen::Vector3d & ecef)
+HomePosition::~HomePosition()
 {
-    return ecef_ref_orientation_ * (ecef - ecef_ref_point_);
+    
 }
 
-void HomePositionManager::wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up)
+QString HomePosition::name(void)
 {
-    Eigen::Vector3d ecef = wgs84ToEcef(lat, lon, alt);
-    Eigen::Vector3d enu = ecefToEnu(ecef);
-    *east = enu.x();
-    *north = enu.y();
-    *up = enu.z();
+    return objectName();
 }
 
-void HomePositionManager::enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
+void HomePosition::setName(const QString& name)
 {
-    *lat=homeLat+y/MEAN_EARTH_DIAMETER*360./PI;
-    *lon=homeLon+x/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
-    *alt=homeAlt+z;
+    setObjectName(name);
+    HomePositionManager::instance()->_storeSettings();
+    emit nameChanged(name);
 }
 
-void HomePositionManager::nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
+QGeoCoordinate HomePosition::coordinate(void)
 {
-    *lat=homeLat+x/MEAN_EARTH_DIAMETER*360./PI;
-    *lon=homeLon+y/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
-    *alt=homeAlt-z;
+    return _coordinate;
 }
 
+void HomePosition::setCoordinate(const QGeoCoordinate& coordinate)
+{
+    _coordinate = coordinate;
+    HomePositionManager::instance()->_storeSettings();
+    emit coordinateChanged(coordinate);
+}

src/HomePositionManager.h

@@ -21,21 +21,41 @@ This file is part of the QGROUNDCONTROL project
 
 ======================================================================*/
 
-#ifndef _UASMANAGER_H_
-#define _UASMANAGER_H_
+#ifndef HomePositionManager_H
+#define HomePositionManager_H
 
-#include "UASInterface.h"
-
-#include <QList>
-#include <QMutex>
+#include "QGCSingleton.h"
+#include "QmlObjectListModel.h"
 
-#include <Eigen/Eigen>
+#include <QGeoCoordinate>
 
-#include "QGCGeo.h"
-#include "QGCSingleton.h"
+class HomePosition : public QObject
+{
+    Q_OBJECT
+    
+public:
+    HomePosition(const QString& name, const QGeoCoordinate& coordinate, QObject* parent = NULL);
+    ~HomePosition();
+    
+    Q_PROPERTY(QString          name        READ name           WRITE setName       NOTIFY nameChanged)
+    Q_PROPERTY(QGeoCoordinate   coordinate  READ coordinate     WRITE setCoordinate NOTIFY coordinateChanged)
+    
+    // Property accessors
+    
+    QString name(void);
+    void setName(const QString& name);
+    
+    QGeoCoordinate coordinate(void);
+    void setCoordinate(const QGeoCoordinate& coordinate);
+    
+signals:
+    void nameChanged(const QString& name);
+    void coordinateChanged(const QGeoCoordinate& coordinate);
+    
+private:
+    QGeoCoordinate  _coordinate;
+};
 
-/// Manages an offline home position as well as performance coordinate transformations
-/// around a home position.
 class HomePositionManager : public QObject
 {
     Q_OBJECT
@@ -43,6 +63,40 @@ class HomePositionManager : public QObject
     DECLARE_QGC_SINGLETON(HomePositionManager, HomePositionManager)
 
 public:
+    Q_PROPERTY(QmlObjectListModel* homePositions READ homePositions CONSTANT)
+    
+    /// If name is not already a home position a new one will be added, otherwise the existing
+    /// home position will be updated
+    Q_INVOKABLE void updateHomePosition(const QString& name, const QGeoCoordinate& coordinate);
+    
+    Q_INVOKABLE void deleteHomePosition(const QString& name);
+    
+    // Property accesors
+    
+    QmlObjectListModel* homePositions(void) { return &_homePositions; }
+    
+    // Should only be called by HomePosition
+    void _storeSettings(void);
+    
+private:
+    /// @brief All access to HomePositionManager singleton is through HomePositionManager::instance
+    HomePositionManager(QObject* parent = NULL);
+    ~HomePositionManager();
+    
+    void _loadSettings(void);
+    
+    QmlObjectListModel  _homePositions;
+    
+    static const char* _settingsGroup;
+    static const char* _latitudeKey;
+    static const char* _longitudeKey;
+    static const char* _altitudeKey;
+    
+// Everything below is deprecated and will be removed once old Map code is removed
+public:
+    
+    // Deprecated methods
+    
     /** @brief Get home position latitude */
     double getHomeLatitude() const {
         return homeLat;
@@ -56,24 +110,10 @@ public:
         return homeAlt;
     }
 
-    /** @brief Get the home position coordinate frame */
-    int getHomeFrame() const
-    {
-        return homeFrame;
-    }
-
-    /** @brief Convert WGS84 coordinates to earth centric frame */
-    Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude);
-    /** @brief Convert earth centric frame to EAST-NORTH-UP frame (x-y-z directions */
-    Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef);
-    /** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */
-    void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up);
-    /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in east-north-up frame */
-    void enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt);
-    /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in north-east-down frame */
-    void nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt);
-
 public slots:
+    
+    // Deprecated methods
+    
     /** @brief Set the current home position, but do not change it on the UAVs */
     bool setHomePosition(double lat, double lon, double alt);
 
@@ -81,11 +121,6 @@ public slots:
     bool setHomePositionAndNotify(double lat, double lon, double alt);
 
 
-    /** @brief Load settings */
-    void loadSettings();
-    /** @brief Store settings */
-    void storeSettings();
-
 signals:
     /** @brief Current home position changed */
     void homePositionChanged(double lat, double lon, double alt);
@@ -94,23 +129,6 @@ protected:
     double homeLat;
     double homeLon;
     double homeAlt;
-    int homeFrame;
-    Eigen::Quaterniond ecef_ref_orientation_;
-    Eigen::Vector3d ecef_ref_point_;
-
-    void initReference(const double & latitude, const double & longitude, const double & altitude);
-    
-private:
-    /// @brief All access to HomePositionManager singleton is through HomePositionManager::instance
-    HomePositionManager(QObject* parent = NULL);
-    ~HomePositionManager();
-
-public:
-    /* Need to align struct pointer to prevent a memory assertion:
-     * See http://eigen.tuxfamily.org/dox-devel/TopicUnalignedArrayAssert.html
-     * for details
-     */
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-#endif // _UASMANAGER_H_
+#endif

src/QmlControls/QmlObjectListModel.cc

@@ -29,6 +29,7 @@
 #include <QDebug>
 
 const int QmlObjectListModel::ObjectRole = Qt::UserRole;
+const int QmlObjectListModel::TextRole = Qt::UserRole + 1;
 
 QmlObjectListModel::QmlObjectListModel(QObject* parent)
     : QAbstractListModel(parent)
@@ -60,6 +61,8 @@ QVariant QmlObjectListModel::data(const QModelIndex &index, int role) const
     
     if (role == ObjectRole) {
         return QVariant::fromValue(_objectList[index.row()]);
+    } else if (role == TextRole) {
+        return QVariant::fromValue(_objectList[index.row()]->objectName());
     } else {
         return QVariant();
     }
@@ -70,6 +73,7 @@ QHash<int, QByteArray> QmlObjectListModel::roleNames(void) const
     QHash<int, QByteArray> hash;
     
     hash[ObjectRole] = "object";
+    hash[TextRole] = "text";
     
     return hash;
 }

src/QmlControls/QmlObjectListModel.h

@@ -65,6 +65,7 @@ private:
     QList<QObject*> _objectList;
         
     static const int ObjectRole;
+    static const int TextRole;
 };
 
 #endif

src/comm/QGCFlightGearLink.cc

@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QMessageBox>
 
 #include <iostream>
+#include <Eigen/Eigen>
 
 #include "QGCFlightGearLink.h"
 #include "QGC.h"

src/comm/QGCXPlaneLink.cc

@@ -33,10 +33,13 @@ This file is part of the QGROUNDCONTROL project
 #include <QDebug>
 #include <QMutexLocker>
 #include <QNetworkInterface>
+#include <QHostInfo>
+
 #include <iostream>
+#include <Eigen/Eigen>
+
 #include "QGCXPlaneLink.h"
 #include "QGC.h"
-#include <QHostInfo>
 #include "UAS.h"
 #include "UASInterface.h"
 #include "QGCMessageBox.h"

src/uas/UAS.cc

@@ -2784,9 +2784,8 @@ void UAS::home()
     double latitude = HomePositionManager::instance()->getHomeLatitude();
     double longitude = HomePositionManager::instance()->getHomeLongitude();
     double altitude = HomePositionManager::instance()->getHomeAltitude();
-    int frame = HomePositionManager::instance()->getHomeFrame();
 
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, MAV_FRAME_GLOBAL, 0, latitude, longitude, altitude);
     _vehicle->sendMessage(msg);
 }
 

src/ui/MainWindow.cc

@@ -609,7 +609,6 @@ void MainWindow::closeEvent(QCloseEvent *event)
     
     _storeCurrentViewState();
     storeSettings();
-    HomePositionManager::instance()->storeSettings();
     event->accept();
 }