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

GCS/OPMap-fixes math error which was causing wrong wp location

Browse Source 
and high processor usage
This commit is contained in:
PT_Dreamer 2012-07-07 16:05:28 +01:00
parent 61dea64a76
commit 42d06ef7d9
2 changed files with 30 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -217,14 +217,14 @@ Point PureProjection::FromLatLngToPixel(const PointLatLng &p,const int &zoom)
}
double PureProjection::courseBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2)
{
double lon1=p1.Lng()* (PI / 180);;
double lat1=p1.Lat()* (PI / 180);;
double lon2=p2.Lng()* (PI / 180);;
double lat2=p2.Lat()* (PI / 180);;
double lon1=p1.Lng()* (M_PI / 180);;
double lat1=p1.Lat()* (M_PI / 180);;
double lon2=p2.Lng()* (M_PI / 180);;
double lat2=p2.Lat()* (M_PI / 180);;
return 2*M_PI-myfmod(atan2(sin(lon1-lon2)*cos(lat2),
cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*PI);
cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*M_PI);
}
double PureProjection::DistanceBetweenLatLng(PointLatLng const& p1,PointLatLng const& p2)

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

@ -266,6 +266,10 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(fals
if(altitude==value)
return;
altitude=value;
if(myHome)
{
relativeCoord.altitudeRelative=altitude-myHome->Altitude();
}
RefreshToolTip();
emit WPValuesChanged(this);
this->update();
@ -273,42 +277,47 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(fals
void WayPointItem::setRelativeCoord(distBearingAltitude value)
{
if(qAbs(value.altitudeRelative-relativeCoord.altitudeRelative<0.0001)
&& qAbs(value.bearing-relativeCoord.bearing<0.0001) && value.distance==relativeCoord.distance)
qDebug()<<"SetRelative("<<value.distance<<","<<value.bearing<<")"<<"OLD:"<<relativeCoord.distance<<","<<relativeCoord.bearing;
if(qAbs(value.distance-relativeCoord.distance)<0.1
&& qAbs(value.bearing-relativeCoord.bearing)<0.01 && value.altitudeRelative==relativeCoord.altitudeRelative)
return;
qDebug()<<"setRelative";
qDebug()<<"setRelative values need update";
relativeCoord=value;
if(myHome)
{
coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing);
SetCoord(map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing));
SetAltitude(myHome->Altitude()+relativeCoord.altitudeRelative);
}
RefreshPos();
RefreshToolTip();
emit WPValuesChanged(this);
this->update();
qDebug()<<"setRelativeCoord EXIT";
}
void WayPointItem::SetCoord(const internals::PointLatLng &value)
{
if(qAbs(Coord().Lat()-value.Lat()<0.0001 && qAbs(Coord().Lng()-value.Lng()<0.0001)))
qDebug()<<"1 SetCoord("<<value.Lat()<<","<<value.Lng()<<")"<<"OLD:"<<Coord().Lat()<<","<<Coord().Lng();
if(qAbs(Coord().Lat()-value.Lat())<0.0001 && qAbs(Coord().Lng()-value.Lng())<0.0001)
{
qDebug()<<"2 SetCoord nothing changed returning";
return;
qDebug()<<"setCoord";
bool abs_coord=false;
bool rel_coord=false;
if(coord!=value)
abs_coord=true;
}
qDebug()<<"3 setCoord there were changes";
coord=value;
distBearingAltitude back=relativeCoord;
if(myHome)
map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing);
if(back.distance!=relativeCoord.distance||back.bearing!=relativeCoord.bearing)
rel_coord=true;
if(abs_coord||rel_coord)
emit WPValuesChanged(this);
map->Projection()->offSetFromLatLngs(myHome->Coord(),Coord(),back.distance,back.bearing);
if(qAbs(back.bearing-relativeCoord.bearing)>0.01 || qAbs(back.distance-relativeCoord.distance)>0.1)
{
qDebug()<<"4 setCoord-relative coordinates where also updated";
relativeCoord=back;
}
emit WPValuesChanged(this);
RefreshPos();
RefreshToolTip();
this->update();
qDebug()<<"5 setCoord EXIT";
}
void WayPointItem::SetDescription(const QString &value)
{