rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

GCS - Relative WPs are now working

Browse Source
This commit is contained in:
PT_Dreamer 2012-05-26 21:23:53 +01:00
parent 8fbef19f5b
commit 7a1501eea3
6 changed files with 41 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.cpp
View File

@ -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);
ret.SetLat(asin(sin(origin.Lat())*cos(d)+cos(origin.Lat())*sin(d)*cos(tc)));
double dlon=atan2(sin(tc)*sin(d)*cos(origin.Lat()),cos(d)-sin(origin.Lat())*sin(p1.Lat()));
ret.SetLng(fmod(origin.Lng()-dlon +PI,2*PI )-PI);
double tc=(atan2(dY,dX));
double lat1=p1.Lat()*M_PI/180;
double lon1=p1.Lng()*M_PI/180;
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;
}

5
ground/openpilotgcs/src/libs/opmapcontrol/src/internals/pureprojection.h
View File

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

7
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
View File

@ -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);

9
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h
View File

@ -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

4
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp
View File

@ -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)

2
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -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