rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

GPSposition trail added, quick and dirty copy/paste, but works OK. Needs settings for menus etc.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2204 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas 2010-12-08 19:36:02 +00:00 committed by sambas
parent d8db3087ca
commit c23df70b49
11 changed files with 506 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
ground/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp Normal file
View File

@ -0,0 +1,199 @@
/**
******************************************************************************
*
* @file gpsitem.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief A graphicsItem representing a UAV
* @see The GNU Public License (GPL) Version 3
* @defgroup OPMapWidget
* @{
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "../internals/pureprojection.h"
#include "gpsitem.h"
namespace mapcontrol
{
GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent):map(map),mapwidget(parent),showtrail(true),trailtime(5),traildistance(50),autosetreached(true)
,autosetdistance(100)
{
pic.load(QString::fromUtf8(":/markers/images/mapquad.png"));
// Don't scale but trust the image we are given
// pic=pic.scaled(50,33,Qt::IgnoreAspectRatio);
localposition=map->FromLatLngToLocal(mapwidget->CurrentPosition());
this->setPos(localposition.X(),localposition.Y());
this->setZValue(4);
trail=new QGraphicsItemGroup();
trail->setParentItem(map);
this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
mapfollowtype=UAVMapFollowType::None;
trailtype=UAVTrailType::ByDistance;
timer.start();
// rect=QRectF(0,0,renderer.defaultSize().width()*0.05,renderer.defaultSize().height()*0.05);
}
GPSItem::~GPSItem()
{
delete trail;
}
void GPSItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
Q_UNUSED(option);
Q_UNUSED(widget);
// painter->rotate(-90);
painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic);
// painter->drawRect(QRectF(-pic.width()/2,-pic.height()/2,pic.width()-1,pic.height()-1));
}
QRectF GPSItem::boundingRect()const
{
return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height());
}
void GPSItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude)
{
if(coord.IsEmpty())
lastcoord=coord;
if(coord!=position)
{
if(trailtype==UAVTrailType::ByTimeElapsed)
{
if(timer.elapsed()>trailtime*1000)
{
trail->addToGroup(new TrailItem(position,altitude,Qt::green,this));
timer.restart();
}
}
else if(trailtype==UAVTrailType::ByDistance)
{
if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance)
{
trail->addToGroup(new TrailItem(position,altitude,Qt::green,this));
lastcoord=position;
}
}
coord=position;
this->altitude=altitude;
RefreshPos();
/*if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap||mapfollowtype==UAVMapFollowType::CenterMap)
{
mapwidget->SetCurrentPosition(coord);
}*/
this->update();
/*if(autosetreached)
{
foreach(QGraphicsItem* i,map->childItems())
{
WayPointItem* wp=qgraphicsitem_cast<WayPointItem*>(i);
if(wp)
{
if(Distance3D(wp->Coord(),wp->Altitude())<autosetdistance)
{
wp->SetReached(true);
emit UAVReachedWayPoint(wp->Number(),wp);
}
}
}
}
if(mapwidget->Home!=0)
{
//verify if the UAV is inside the safety bouble
if(Distance3D(mapwidget->Home->Coord(),mapwidget->Home->Altitude())>mapwidget->Home->SafeArea())
{
if(mapwidget->Home->safe!=false)
{
mapwidget->Home->safe=false;
mapwidget->Home->update();
emit UAVLeftSafetyBouble(this->coord);
}
}
else
{
if(mapwidget->Home->safe!=true)
{
mapwidget->Home->safe=true;
mapwidget->Home->update();
}
}
}*/
}
}
/**
* Rotate the UAV Icon on the map, or rotate the map
* depending on the display mode
*/
void GPSItem::SetUAVHeading(const qreal &value)
{
if(mapfollowtype==UAVMapFollowType::CenterAndRotateMap)
{
mapwidget->SetRotate(-value);
}
else {
if (this->rotation() != value)
this->setRotation(value);
}
}
int GPSItem::type()const
{
return Type;
}
void GPSItem::RefreshPos()
{
localposition=map->FromLatLngToLocal(coord);
this->setPos(localposition.X(),localposition.Y());
foreach(QGraphicsItem* i,trail->childItems())
{
TrailItem* w=qgraphicsitem_cast<TrailItem*>(i);
if(w)
w->setPos(map->FromLatLngToLocal(w->coord).X(),map->FromLatLngToLocal(w->coord).Y());
//this->update();
}
}
void GPSItem::SetTrailType(const UAVTrailType::Types &value)
{
trailtype=value;
if(trailtype==UAVTrailType::ByTimeElapsed)
timer.restart();
}
void GPSItem::SetShowTrail(const bool &value)
{
showtrail=value;
trail->setVisible(value);
}
void GPSItem::DeleteTrail()const
{
foreach(QGraphicsItem* i,trail->childItems())
delete i;
}
double GPSItem::Distance3D(const internals::PointLatLng &coord, const int &altitude)
{
return sqrt(pow(internals::PureProjection::DistanceBetweenLatLng(this->coord,coord)*1000,2)+
pow(this->altitude-altitude,2));
}
}

209
ground/src/libs/opmapcontrol/src/mapwidget/gpsitem.h Normal file
View File

@ -0,0 +1,209 @@
/**
******************************************************************************
*
* @file gpsitem.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief A graphicsItem representing a WayPoint
* @see The GNU Public License (GPL) Version 3
* @defgroup OPMapWidget
* @{
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef GPSITEM_H
#define GPSITEM_H
#include <QGraphicsItem>
#include <QPainter>
#include <QLabel>
#include "../internals/pointlatlng.h"
#include "mapgraphicitem.h"
#include "waypointitem.h"
#include <QObject>
#include "uavmapfollowtype.h"
#include "uavtrailtype.h"
#include <QtSvg/QSvgRenderer>
#include "opmapwidget.h"
#include "trailitem.h"
namespace mapcontrol
{
class WayPointItem;
class OPMapWidget;
/**
* @brief A QGraphicsItem representing the UAV
*
* @class UAVItem gpsitem.h "mapwidget/gpsitem.h"
*/
class GPSItem:public QObject,public QGraphicsItem
{
Q_OBJECT
Q_INTERFACES(QGraphicsItem)
public:
enum { Type = UserType + 2 };
GPSItem(MapGraphicItem* map,OPMapWidget* parent);
~GPSItem();
/**
* @brief Sets the UAV position
*
* @param position LatLng point
* @param altitude altitude in meters
*/
void SetUAVPos(internals::PointLatLng const& position,int const& altitude);
/**
* @brief Sets the UAV heading
*
* @param value heading angle (north=0deg)
*/
void SetUAVHeading(qreal const& value);
/**
* @brief Returns the UAV position
*
* @return internals::PointLatLng
*/
internals::PointLatLng UAVPos()const{return coord;}
/**
* @brief Sets the Map follow type
*
* @param value can be "none"(map doesnt follow UAV), "CenterMap"(map moves to keep UAV centered) or "CenterAndRotateMap"(map moves and rotates to keep UAV centered and straight)
*/
void SetMapFollowType(UAVMapFollowType::Types const& value){mapfollowtype=value;}
/**
* @brief Sets the trail type
*
* @param value can be "NoTrail"(no trail is plotted), "ByTimeElapsed"(a trail point is plotted each TrailTime()) or ByDistance(a trail point is plotted if the distance between the UAV and the last trail point is bigger than TrailDistance())
*/
void SetTrailType(UAVTrailType::Types const& value);
/**
* @brief Returns the map follow method used
*
* @return UAVMapFollowType::Types
*/
UAVMapFollowType::Types GetMapFollowType()const{return mapfollowtype;}
/**
* @brief Returns the UAV trail type. It can be plotted by time elapsed or distance
*
* @return UAVTrailType::Types
*/
UAVTrailType::Types GetTrailType()const{return trailtype;}
void paint(QPainter *painter, const QStyleOptionGraphicsItem *option,
QWidget *widget);
void RefreshPos();
QRectF boundingRect() const;
/**
* @brief Sets the trail time to be used if TrailType is ByTimeElapsed
*
* @param seconds the UAV trail time elapsed value. If the trail type is time elapsed
* a trail point will be plotted each "value returned" seconds.
*/
void SetTrailTime(int const& seconds){trailtime=seconds;}
/**
* @brief Returns the UAV trail time elapsed value. If the trail type is time elapsed
* a trail point will be plotted each "value returned" seconds.
*
* @return int
*/
int TrailTime()const{return trailtime;}
/**
* @brief Sets the trail distance to be used if TrailType is ByDistance
*
* @param distance the UAV trail plot distance.
* If the trail type is ByDistance a trail dot is plotted if
* the distance between the current UAV position and the last trail point
* is bigger than the returned value
*/
void SetTrailDistance(int const& distance){traildistance=distance;}
/**
* @brief Returns the UAV trail plot distance.
* If the trail type is distance diference a trail dot is plotted if
* the distance between the current UAV position and the last trail point
* is bigger than the returned value
*
* @return int
*/
int TrailDistance()const{return traildistance;}
/**
* @brief Returns true if UAV trail is shown
*
* @return bool
*/
bool ShowTrail()const{return showtrail;}
/**
* @brief Used to define if the UAV displays a trail
*
* @param value
*/
void SetShowTrail(bool const& value);
/**
* @brief Deletes all the trail points
*/
void DeleteTrail()const;
/**
* @brief Returns true if the UAV automaticaly sets WP reached value (changing its color)
*
* @return bool
*/
bool AutoSetReached()const{return autosetreached;}
/**
* @brief Defines if the UAV can set the WP's "reached" value automaticaly.
*
* @param value
*/
void SetAutoSetReached(bool const& value){autosetreached=value;}
/**
* @brief Returns the 3D distance in meters necessary for the UAV to set WP's to "reached"
*
* @return double
*/
double AutoSetDistance()const{return autosetdistance;}
/**
* @brief Sets the the 3D distance in meters necessary for the UAV to set WP's to "reached"
*
* @param value
*/
void SetAutoSetDistance(double const& value){autosetdistance=value;}
int type() const;
private:
MapGraphicItem* map;
int altitude;
UAVMapFollowType::Types mapfollowtype;
UAVTrailType::Types trailtype;
internals::PointLatLng coord;
internals::PointLatLng lastcoord;
QPixmap pic;
core::Point localposition;
OPMapWidget* mapwidget;
QGraphicsItemGroup* trail;
QTime timer;
bool showtrail;
int trailtime;
int traildistance;
bool autosetreached;
double Distance3D(internals::PointLatLng const& coord, int const& altitude);
double autosetdistance;
// QRectF rect;
public slots:
signals:
void UAVReachedWayPoint(int const& waypointnumber,WayPointItem* waypoint);
void UAVLeftSafetyBouble(internals::PointLatLng const& position);
};
}
#endif // GPSITEM_H

8
ground/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp
View File

@ -25,6 +25,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "uavitem.h"
#include "gpsitem.h"
#include "homeitem.h"
#include "mapgraphicitem.h"
@ -87,7 +88,9 @@ namespace mapcontrol
HomeItem* www=qgraphicsitem_cast<HomeItem*>(i);
if(www)
www->RefreshPos();
GPSItem* wwww=qgraphicsitem_cast<GPSItem*>(i);
if(wwww)
wwww->RefreshPos();
}
}
void MapGraphicItem::ChildPosRefresh()
@ -103,6 +106,9 @@ namespace mapcontrol
HomeItem* www=qgraphicsitem_cast<HomeItem*>(i);
if(www)
www->RefreshPos();
GPSItem* wwww=qgraphicsitem_cast<GPSItem*>(i);
if(wwww)
wwww->RefreshPos();
}
}
void MapGraphicItem::ConstructLastImage(int const& zoomdiff)

2
ground/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro
View File

@ -9,6 +9,7 @@ SOURCES += mapgraphicitem.cpp \
configuration.cpp \
waypointitem.cpp \
uavitem.cpp \
gpsitem.cpp \
trailitem.cpp \
homeitem.cpp \
mapripform.cpp \
@ -22,6 +23,7 @@ HEADERS += mapgraphicitem.h \
configuration.h \
waypointitem.h \
uavitem.h \
gpsitem.h \
uavmapfollowtype.h \
uavtrailtype.h \
trailitem.h \

16
ground/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
View File

@ -33,7 +33,7 @@
namespace mapcontrol
{
OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config):QGraphicsView(parent),configuration(config),UAV(0),Home(0),followmouse(true),compass(0),showuav(false),showhome(false)
OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config):QGraphicsView(parent),configuration(config),UAV(0),GPS(0),Home(0),followmouse(true),compass(0),showuav(false),showhome(false)
{
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
core=new internals::Core;
@ -73,6 +73,20 @@ namespace mapcontrol
}
}
if(value && GPS==0)
{
GPS=new GPSItem(map,this);
GPS->setParentItem(map);
}
else if(!value)
{
if(GPS!=0)
{
delete GPS;
GPS=0;
}
}
}
void OPMapWidget::SetShowHome(const bool &value)
{

3
ground/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h
View File

@ -37,11 +37,13 @@
#include "waypointitem.h"
#include "QtSvg/QGraphicsSvgItem"
#include "uavitem.h"
#include "gpsitem.h"
#include "homeitem.h"
#include "mapripper.h"
namespace mapcontrol
{
class UAVItem;
class GPSItem;
class HomeItem;
/**
* @brief Collection of static functions to help dealing with various enums used
@ -338,6 +340,7 @@ namespace mapcontrol
void SetShowCompass(bool const& value);
UAVItem* UAV;
GPSItem* GPS;
HomeItem* Home;
void SetShowUAV(bool const& value);
bool ShowUAV()const{return showuav;}

5
ground/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp
View File

@ -28,8 +28,9 @@
#include <QDateTime>
namespace mapcontrol
{
TrailItem::TrailItem(internals::PointLatLng const& coord,int const& altitude,QGraphicsItem* parent):QGraphicsItem(parent),coord(coord)
TrailItem::TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color, QGraphicsItem* parent):QGraphicsItem(parent),coord(coord)
{
m_brush=color;
QDateTime time=QDateTime::currentDateTime();
QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6);
setToolTip(QString(tr("Position:")+"%1\n"+tr("Altitude:")+"%2\n"+tr("Time:")+"%3").arg(coord_str).arg(QString::number(altitude)).arg(time.toString()));
@ -38,7 +39,7 @@ namespace mapcontrol
void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
// painter->drawRect(QRectF(-3,-3,6,6));
painter->setBrush(Qt::red);
painter->setBrush(m_brush);
painter->drawEllipse(-2,-2,4,4);
}
QRectF TrailItem::boundingRect()const

3
ground/src/libs/opmapcontrol/src/mapwidget/trailitem.h
View File

@ -42,13 +42,14 @@ namespace mapcontrol
Q_INTERFACES(QGraphicsItem)
public:
enum { Type = UserType + 3 };
TrailItem(internals::PointLatLng const& coord,int const& altitude,QGraphicsItem* parent);
TrailItem(internals::PointLatLng const& coord,int const& altitude, QBrush color, QGraphicsItem* parent);
void paint(QPainter *painter, const QStyleOptionGraphicsItem *option,
QWidget *widget);
QRectF boundingRect() const;
int type() const;
internals::PointLatLng coord;
private:
QBrush m_brush;
public slots:

4
ground/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp
View File

@ -77,7 +77,7 @@ namespace mapcontrol
{
if(timer.elapsed()>trailtime*1000)
{
trail->addToGroup(new TrailItem(position,altitude,this));
trail->addToGroup(new TrailItem(position,altitude,Qt::red,this));
timer.restart();
}
@ -86,7 +86,7 @@ namespace mapcontrol
{
if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance)
{
trail->addToGroup(new TrailItem(position,altitude,this));
trail->addToGroup(new TrailItem(position,altitude,Qt::red,this));
lastcoord=position;
}
}

63
ground/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -108,6 +108,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
// current position
getUAV_LLA(latitude, longitude, altitude);
//getGPS_LLA(latitude, longitude, altitude);
internals::PointLatLng pos_lat_lon = internals::PointLatLng(latitude, longitude);
@ -166,6 +167,11 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_map->UAV->SetTrailType(UAVTrailType::ByTimeElapsed);
// m_map->UAV->SetTrailType(UAVTrailType::ByDistance);
m_map->GPS->SetTrailTime(uav_trail_time_list[0]); // seconds
m_map->GPS->SetTrailDistance(uav_trail_distance_list[1]); // meters
m_map->GPS->SetTrailType(UAVTrailType::ByTimeElapsed);
// m_map->GPS->SetTrailType(UAVTrailType::ByDistance);
// **************
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
@ -276,6 +282,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
m_map->SetCurrentPosition(home_position.coord); // set the map position
m_map->Home->SetCoord(home_position.coord); // set the HOME position
m_map->UAV->SetUAVPos(home_position.coord, 0.0); // set the UAV position
m_map->GPS->SetUAVPos(home_position.coord, 0.0); // set the UAV position
// **************
// create various context menu (mouse right click menu) actions
@ -637,6 +644,15 @@ void OPMapGadgetWidget::updatePosition()
m_map->UAV->SetUAVPos(uav_pos, uav_altitude_meters); // set the maps UAV position
m_map->UAV->SetUAVHeading(uav_heading_degrees); // set the maps UAV heading
if (!getGPS_LLA(latitude, longitude, altitude))
return;
uav_pos = internals::PointLatLng(latitude, longitude); // current UAV position
m_map->GPS->SetUAVPos(uav_pos, uav_altitude_meters); // set the maps UAV position
m_map->GPS->SetUAVHeading(uav_heading_degrees); // set the maps UAV heading
}
/**
@ -1575,6 +1591,7 @@ void OPMapGadgetWidget::onShowUAVAct_toggled(bool show)
return;
m_map->UAV->setVisible(show);
m_map->GPS->setVisible(show);
}
void OPMapGadgetWidget::onMapModeActGroup_triggered(QAction *action)
@ -1686,6 +1703,7 @@ void OPMapGadgetWidget::onClearUAVtrailAct_triggered()
return;
m_map->UAV->DeleteTrail();
m_map->GPS->DeleteTrail();
}
void OPMapGadgetWidget::onUAVTrailTimeActGroup_triggered(QAction *action)
@ -2187,6 +2205,51 @@ bool OPMapGadgetWidget::getUAV_LLA(double &latitude, double &longitude, double &
return true;
}
// *************************************************************************************
bool OPMapGadgetWidget::getGPS_LLA(double &latitude, double &longitude, double &altitude)
{
UAVObject *obj;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
if (!pm) return false;
UAVObjectManager *om = pm->getObject<UAVObjectManager>();
if (!om) return false;
obj = dynamic_cast<UAVDataObject*>(om->getObject(QString("GPSPosition")));
if (!obj) return false;
latitude = obj->getField(QString("Latitude"))->getDouble();
longitude = obj->getField(QString("Longitude"))->getDouble();
altitude = obj->getField(QString("Altitude"))->getDouble();
latitude *= 1E-7;
longitude *= 1E-7;
if (latitude != latitude) latitude = 0; // nan detection
// if (isNan(latitude)) latitude = 0; // nan detection
else
// if (!isFinite(latitude)) latitude = 0;
// else
if (latitude > 90) latitude = 90;
else
if (latitude < -90) latitude = -90;
if (longitude != longitude) longitude = 0; // nan detection
else
// if (longitude > std::numeric_limits<double>::max()) longitude = 0; // +infinite
// else
// if (longitude < -std::numeric_limits<double>::max()) longitude = 0; // -infinite
// else
if (longitude > 180) longitude = 180;
else
if (longitude < -180) longitude = -180;
if (altitude != altitude) altitude = 0; // nan detection
return true;
}
double OPMapGadgetWidget::getUAV_Yaw()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();

1
ground/src/plugins/opmap/opmapgadgetwidget.h
View File

@ -322,6 +322,7 @@ private:
internals::PointLatLng destPoint(internals::PointLatLng source, double bear, double dist);
bool getUAV_LLA(double &latitude, double &longitude, double &altitude);
bool getGPS_LLA(double &latitude, double &longitude, double &altitude);
double getUAV_Yaw();
void setMapFollowingMode();