GCS - Relative WPs are now working

ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp

@@ -243,15 +243,27 @@ Point PureProjection::FromLatLngToPixel(const PointLatLng &p,const int &zoom)
         dY=DistanceBetweenLatLng(p1,p2)*sin(courseBetweenLatLng(p1,p2));
     }
 
+    double PureProjection::myfmod(double x,double y)
+    {
+        return x - y*floor(x/y);
+    }
+
     PointLatLng PureProjection::translate(PointLatLng  p1,double dX,double dY)
     {
-        PointLatLng origin=p1;
         PointLatLng ret;
         double d=sqrt(pow(dX,2)+pow(dY,2));
-        double tc=atan2(dY,dX);
+        double tc=(atan2(dY,dX));
-        ret.SetLat(asin(sin(origin.Lat())*cos(d)+cos(origin.Lat())*sin(d)*cos(tc)));
+        double lat1=p1.Lat()*M_PI/180;
-        double dlon=atan2(sin(tc)*sin(d)*cos(origin.Lat()),cos(d)-sin(origin.Lat())*sin(p1.Lat()));
+        double lon1=p1.Lng()*M_PI/180;
-        ret.SetLng(fmod(origin.Lng()-dlon +PI,2*PI )-PI);
+        double R=6378137;
+        double lat2 = asin(sin(lat1)*cos(d/R) + cos(lat1)*sin(d/R)*cos(tc) );
+        qDebug()<<lat2<<lat1;
+        double lon2 = lon1 + atan2(sin(tc)*sin(d/R)*cos(lat1),
+                             cos(d/R)-sin(lat1)*sin(lat2));
+        lat2=lat2*180/M_PI;
+        lon2=lon2*180/M_PI;
+        ret.SetLat(lat2);
+        ret.SetLng(lon2);
         return ret;
     }
 

ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h

@@ -33,7 +33,7 @@
 #include "pointlatlng.h"
 #include "cmath"
 #include "rectlatlng.h"
-
+#include <QDebug>
 using namespace core;
 
 namespace internals
@@ -106,7 +106,8 @@ protected:
     static double e3fn(const double &x);
     static double mlfn(const double &e0,const double &e1,const double &e2,const double &e3,const double &phi);
     static qlonglong GetUTMzone(const double &lon);
-
+private:
+    double myfmod(double x, double y);
 };
 }
 

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp

@@ -211,6 +211,13 @@ namespace mapcontrol
         item->setParentItem(map);
         return item;
     }
+    WayPointItem* OPMapWidget::WPCreate(const QPoint &relativeCoord, const int &altitude, const QString &description)
+    {
+        WayPointItem* item=new WayPointItem(relativeCoord,altitude,description,map);
+        ConnectWP(item);
+        item->setParentItem(map);
+        return item;
+    }
     WayPointItem* OPMapWidget::WPInsert(const int &position)
     {
         WayPointItem* item=new WayPointItem(this->CurrentPosition(),0,map);

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h

@@ -279,6 +279,15 @@ namespace mapcontrol
         */
         WayPointItem* WPCreate(internals::PointLatLng const& coord,int const& altitude, QString const& description);
         /**
+        * @brief Creates a new WayPoint
+        *
+        * @param coord the offset in meters to the home position
+        * @param altitude the Altitude of the WayPoint
+        * @param description the description of the WayPoint
+        * @return WayPointItem a pointer to the WayPoint created
+        */
+        WayPointItem *WPCreate(const QPoint &relativeCoord, const int &altitude, const QString &description);
+        /**
         * @brief Inserts a new WayPoint on the specified position
         *
         * @param position index of the WayPoint

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp

@@ -278,6 +278,10 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu
         if(myType==relative)
         {
             coord=map->Projection()->translate(homepos,relativeCoord.x(),relativeCoord.y());
+            emit WPValuesChanged(this);
+            RefreshPos();
+            RefreshToolTip();
+            this->update();
         }
     }
     void WayPointItem::WPRenumbered(const int &oldnumber, const int &newnumber, WayPointItem *waypoint)

ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp

@@ -271,7 +271,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
 	m_map->UAV->SetUAVPos(m_home_position.coord, 0.0);        // set the UAV position
     if(m_map->GPS)
         m_map->GPS->SetUAVPos(m_home_position.coord, 0.0);        // set the UAV position
-    m_map->WPCreate();
+    m_map->WPCreate(QPoint(1000,1000),10,"aaa");
     // **************
     // create various context menu (mouse right click menu) actions