From 64ba15cf07290d41c0ff4e05e8acc1b4cd745f10 Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Sat, 2 Jun 2012 14:29:22 -0500
Subject: [PATCH] Add the ability to add waypoints via OPMap

---
 .../src/plugins/opmap/opmapgadgetwidget.cpp   | 39 +++----------
 .../src/plugins/opmap/pathcompiler.cpp        | 55 ++++++++++++++++++-
 2 files changed, 61 insertions(+), 33 deletions(-)

diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
index 031eb6079..da41019e7 100644
--- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
@@ -2,7 +2,7 @@
  ******************************************************************************
  *
  * @file       opmapgadgetwidget.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  * @addtogroup GCSPlugins GCS Plugins
  * @{
  * @addtogroup OPMapPlugin OpenPilot Map Plugin
@@ -1898,42 +1898,19 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
 
 void OPMapGadgetWidget::onAddWayPointAct_triggered()
 {
+    Q_ASSERT(m_widget);
+    Q_ASSERT(m_map);
+
     if (!m_widget || !m_map)
         return;
 
     if (m_map_mode != Normal_MapMode)
         return;
 
-    m_waypoint_list_mutex.lock();
-
-    // create a waypoint on the map at the last known mouse position
-    t_waypoint *wp = new t_waypoint;
-    wp->map_wp_item = NULL;
-    wp->coord = m_context_menu_lat_lon;
-    wp->altitude = 0;
-    wp->description = "";
-    wp->locked = false;
-    wp->time_seconds = 0;
-    wp->hold_time_seconds = 0;
-    wp->map_wp_item = m_map->WPCreate(wp->coord, wp->altitude, wp->description);
-
-    wp->map_wp_item->setZValue(10 + wp->map_wp_item->Number());
-
-    wp->map_wp_item->setFlag(QGraphicsItem::ItemIsMovable, !wp->locked);
-
-    if (wp->map_wp_item)
-    {
-        if (!wp->locked)
-            wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png"));
-        else
-            wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png"));
-        wp->map_wp_item->update();
-    }
-
-    // and remember it in our own local waypoint list
-    m_waypoint_list.append(wp);
-
-    m_waypoint_list_mutex.unlock();
+    struct PathCompiler::waypoint newWaypoint;
+    newWaypoint.latitude = m_context_menu_lat_lon.Lat();
+    newWaypoint.longitude = m_context_menu_lat_lon.Lng();
+    pathCompiler->doAddWaypoint(newWaypoint);
 }
 
 /**
diff --git a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
index 640c75ade..fc28c95a4 100644
--- a/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
@@ -93,9 +93,28 @@ int PathCompiler::loadPath(QString filename)
   * @param waypoint the new waypoint to add
   * @param position which position to insert it to, defaults to end
   */
-void PathCompiler::doAddWaypoint(struct PathCompiler::waypoint, int /* position */)
+void PathCompiler::doAddWaypoint(struct PathCompiler::waypoint waypoint, int /*position*/)
 {
+    UAVObjectManager *objMngr;
+    objMngr = getObjectManager();
+    if (objMngr == NULL)
+        return;
 
+    /* TODO: If a waypoint is inserted not at the end shift them all by one and */
+    /* add the data there */
+
+    Waypoint *obj = new Waypoint();
+    Q_ASSERT(obj);
+    if (obj) {
+        // Register a new waypoint instance
+        quint32 newInstId = objMngr->getNumInstances(obj->getObjID());
+        obj->initialize(newInstId,obj->getMetaObject());
+        objMngr->registerObject(obj);
+
+        // Set the data in the new object
+        Waypoint::DataFields newWaypoint = InternalToUavo(waypoint);
+        obj->setData(newWaypoint);
+    }
 }
 
 /**
@@ -181,9 +200,41 @@ struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields
   * @param internal The internal structure type
   * @returns The waypoint UAVO data structure
   */
-Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint /*internal*/)
+Waypoint::DataFields PathCompiler::InternalToUavo(struct waypoint internal)
 {
     Waypoint::DataFields uavo;
+
+    double homeLLA[3];
+    double LLA[3];
+    double NED[3];
+    struct PathCompiler::waypoint internalWaypoint;
+
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+    Q_ASSERT(homeLocation);
+    if (homeLocation == NULL)
+        return uavo;
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
+    homeLLA[0] = homeLocationData.Latitude / 10e6;
+    homeLLA[1] = homeLocationData.Longitude / 10e6;
+    homeLLA[2] = homeLocationData.Altitude;
+
+    // TODO: Give the point a concept of altitude
+    LLA[0] = internal.latitude;
+    LLA[1] = internal.longitude;
+    LLA[2] = -50;
+
+    Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED);
+
+    uavo.Position[Waypoint::POSITION_NORTH] = NED[0];
+    uavo.Position[Waypoint::POSITION_EAST] = NED[1];
+    uavo.Position[Waypoint::POSITION_DOWN] = NED[2];
+
+    uavo.Action = Waypoint::ACTION_NEXT;
+
+    uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5;
+    uavo.Velocity[Waypoint::VELOCITY_EAST] = 0;
+    uavo.Velocity[Waypoint::VELOCITY_DOWN] = 0;
+
     return uavo;
 }