mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Added support for trend lines and text info on map.
This commit is contained in:
Kenz Dale
parent
1d42b64965
commit
3b9e3c0054
@ -26,12 +26,13 @@
|
||||
*/
|
||||
#include "../internals/pureprojection.h"
|
||||
#include "uavitem.h"
|
||||
|
||||
const qreal Pi = 3.14;
|
||||
|
||||
namespace mapcontrol
|
||||
{
|
||||
UAVItem::UAVItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true)
|
||||
,autosetdistance(100)
|
||||
,autosetdistance(100),altitude(0)
|
||||
{
|
||||
pic.load(uavPic);
|
||||
this->setFlag(QGraphicsItem::ItemIsMovable,true);
|
||||
@ -57,18 +58,35 @@ namespace mapcontrol
|
||||
{
|
||||
Q_UNUSED(option);
|
||||
Q_UNUSED(widget);
|
||||
QLineF line(0,0,1.0,1.0);
|
||||
|
||||
|
||||
QPen myPen;
|
||||
QColor myColor(Qt::red);
|
||||
painter->setPen(myPen);
|
||||
line.setP1(QPointF(0,0));
|
||||
line.setLength(60.0);
|
||||
line.setAngle(90.0);
|
||||
myPen.setColor(myColor);
|
||||
|
||||
//Turn on anti-aliasing so the fonts don't look terrible
|
||||
painter->setRenderHint(QPainter::Antialiasing, true);
|
||||
|
||||
qreal arrowSize = 10;
|
||||
|
||||
//Set pen attributes
|
||||
QColor myColor(Qt::red);
|
||||
myPen.setWidth(3);
|
||||
myPen.setColor(myColor);
|
||||
painter->setPen(myPen);
|
||||
|
||||
//Set brush attributes
|
||||
painter->setBrush(myColor);
|
||||
|
||||
//Create line from (0,0), to (1,1). Later, we'll scale and rotate it
|
||||
QLineF line(0,0,1.0,1.0);
|
||||
|
||||
//Set the starting point to (0,0)
|
||||
line.setP1(QPointF(0,0));
|
||||
|
||||
//Set angle and length
|
||||
line.setLength(60.0);
|
||||
line.setAngle(90.0);
|
||||
|
||||
//Form arrowhead
|
||||
double angle = ::acos(line.dx() / line.length());
|
||||
if (line.dy() <= 0)
|
||||
angle = (Pi * 2) - angle;
|
||||
@ -77,15 +95,89 @@ namespace mapcontrol
|
||||
cos(angle + Pi / 3) * arrowSize);
|
||||
QPointF arrowP2 = line.pointAt(1) + QPointF(sin(angle + Pi - Pi / 3) * arrowSize,
|
||||
cos(angle + Pi - Pi / 3) * arrowSize);
|
||||
|
||||
//Generate arrowhead
|
||||
arrowHead.clear();
|
||||
arrowHead << line.pointAt(1) << arrowP1 << arrowP2;
|
||||
painter->drawPolygon(arrowHead);
|
||||
painter->setPen(myPen);
|
||||
painter->drawLine(line);
|
||||
|
||||
//*********** Create trend arc
|
||||
float radius;
|
||||
|
||||
float spanAngle = yawRate_dps * 3; //Forecast 3 seconds into the future
|
||||
|
||||
float meters2pixels=5; //This should be a function of the zoom level, and not a fixed constant
|
||||
radius=fabs((groundspeed_kph/3.6)/(yawRate_dps*Pi/180))*meters2pixels; //Calculate radius in [m], and then convert to Px.
|
||||
// qDebug()<< "Radius:" << radius;
|
||||
// qDebug()<< "Span angle:" << spanAngle;
|
||||
|
||||
//Set trend arc's color
|
||||
myPen.setColor(Qt::magenta);
|
||||
painter->setPen(myPen);
|
||||
|
||||
//Draw arc. Qt is incomprehensibly limited in that it does not let you set a radius and two points,
|
||||
//so instead it's a hackish approach requiring a rectangle and a span angle
|
||||
if (spanAngle > 0){
|
||||
QRectF rect(0, -radius, radius*2, radius*2);
|
||||
painter->drawArc(rect, 180*16, -spanAngle*16);
|
||||
}
|
||||
else{
|
||||
QRectF rect(-2*radius, -radius, radius*2, radius*2);
|
||||
painter->drawArc(rect, 0*16, -spanAngle*16);
|
||||
}
|
||||
|
||||
//HUH? What does this do?
|
||||
painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic);
|
||||
|
||||
//***** Text info overlay. The font is a "glow" font, so that it's easier to use on the map
|
||||
|
||||
//Rotate the text back to vertical
|
||||
qreal rot=this->rotation();
|
||||
painter->rotate(-1*rot);
|
||||
painter->drawText(QPointF(10,10),"KENZ");
|
||||
|
||||
//Define font
|
||||
QFont borderfont( "Arial", 14, QFont::Normal, false );
|
||||
|
||||
//Top left corner of text
|
||||
int textAnchorX = 20;
|
||||
int textAnchorY = 20;
|
||||
|
||||
//Create text lines
|
||||
QString uavoInfoStrLine1, uavoInfoStrLine2;
|
||||
QString uavoInfoStrLine3;
|
||||
QString uavoInfoStrLine4;
|
||||
|
||||
//For whatever reason, Qt does not let QPainterPath have text wrapping. So each line of
|
||||
//text has to be added to a different line.
|
||||
uavoInfoStrLine1.append(QString("CAS: %1 kph").arg(CAS_mps));
|
||||
uavoInfoStrLine2.append(QString("Groundspeed: %1 kph").arg(groundspeed_kph));
|
||||
uavoInfoStrLine3.append(QString("Lat-Lon: %1, %2").arg(coord.Lat()).arg(coord.Lng()));
|
||||
uavoInfoStrLine4.append(QString("Altitude: %1 m").arg(this->altitude));
|
||||
|
||||
//Add the lines of text to the path
|
||||
//NOTE: We must use QPainterPath for the outlined text font. QPaint does not support this.
|
||||
QPainterPath path;
|
||||
path.addText(textAnchorX, textAnchorY+16*0, borderfont, uavoInfoStrLine1);
|
||||
path.addText(textAnchorX, textAnchorY+16*1, borderfont, uavoInfoStrLine2);
|
||||
path.addText(textAnchorX, textAnchorY+16*2, borderfont, uavoInfoStrLine3);
|
||||
path.addText(textAnchorX, textAnchorY+16*3, borderfont, uavoInfoStrLine4);
|
||||
|
||||
//First pass is the outline...
|
||||
myPen.setWidth(4);
|
||||
myPen.setColor(Qt::black);
|
||||
painter->setPen(myPen);
|
||||
painter->setBrush(Qt::black);
|
||||
painter->drawPath(path);
|
||||
|
||||
//...second pass is the inlay
|
||||
myPen.setWidth(1);
|
||||
myPen.setColor(Qt::white);
|
||||
painter->setBrush(Qt::white);
|
||||
painter->setPen(myPen);
|
||||
painter->drawPath(path);
|
||||
|
||||
}
|
||||
|
||||
QRectF UAVItem::boundingRect()const
|
||||
@ -93,6 +185,36 @@ namespace mapcontrol
|
||||
return QRectF(-pic.width()/2,-pic.height()/2,pic.width(),pic.height());
|
||||
}
|
||||
|
||||
void UAVItem::SetNED(double NED[3]){
|
||||
this->NED[0] = NED[0];
|
||||
this->NED[1] = NED[1];
|
||||
this->NED[2] = NED[2];
|
||||
}
|
||||
|
||||
void UAVItem::SetYawRate(double yawRate_dps){
|
||||
this->yawRate_dps=yawRate_dps;
|
||||
|
||||
//There is a minimum arc distance under which Qt no longer draws an arc. At this low a turning rate,
|
||||
//all curves look straight, so the user won't notice the difference. Moreover, our sensors aren't
|
||||
//accurate enough to reliably describe this low a yaw rate.
|
||||
if (fabs(this->yawRate_dps) < 5e-1){ //This number is really the smallest we can go. Any smaller, and it might have problems if we forecast a shorter distance into the future
|
||||
this->yawRate_dps=5e-1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void UAVItem::SetCAS(double CAS_mps){
|
||||
this->CAS_mps=CAS_mps;
|
||||
}
|
||||
|
||||
void UAVItem::SetGroundspeed(double vNED[3]){
|
||||
this->vNED[0] = vNED[0];
|
||||
this->vNED[1] = vNED[1];
|
||||
this->vNED[2] = vNED[2];
|
||||
groundspeed_kph=sqrt(vNED[0]*vNED[0] + vNED[1]*vNED[1] + vNED[2]*vNED[2])*3.6;
|
||||
}
|
||||
|
||||
|
||||
void UAVItem::SetUAVPos(const internals::PointLatLng &position, const int &altitude)
|
||||
{
|
||||
if(coord.IsEmpty())
|
||||
|
||||
@ -57,6 +57,31 @@ namespace mapcontrol
|
||||
enum { Type = UserType + 2 };
|
||||
UAVItem(MapGraphicItem* map,OPMapWidget* parent, QString uavPic=QString::fromUtf8(":/uavs/images/mapquad.png"));
|
||||
~UAVItem();
|
||||
|
||||
/**
|
||||
* @brief Sets the UAV NED position
|
||||
*
|
||||
* @param NED
|
||||
*/
|
||||
void SetNED(double NED[3]);
|
||||
/**
|
||||
* @brief Sets the UAV groundspeed
|
||||
*
|
||||
* @param NED
|
||||
*/
|
||||
void SetGroundspeed(double vNED[3]);
|
||||
/**
|
||||
* @brief Sets the UAV Calibrated Airspeed
|
||||
*
|
||||
* @param NED
|
||||
*/
|
||||
void SetCAS(double CAS);
|
||||
/**
|
||||
* @brief Sets the UAV yaw rate
|
||||
*
|
||||
* @param NED
|
||||
*/
|
||||
void SetYawRate(double yawRate_dps);
|
||||
/**
|
||||
* @brief Sets the UAV position
|
||||
*
|
||||
@ -201,6 +226,11 @@ namespace mapcontrol
|
||||
UAVTrailType::Types trailtype;
|
||||
internals::PointLatLng coord;
|
||||
internals::PointLatLng lastcoord;
|
||||
double NED[3];
|
||||
double vNED[3];
|
||||
double CAS_mps;
|
||||
double groundspeed_kph;
|
||||
double yawRate_dps;
|
||||
QPixmap pic;
|
||||
core::Point localposition;
|
||||
OPMapWidget* mapwidget;
|
||||
|
||||
@ -390,9 +390,10 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
|
||||
//attRaw->setData(rawData);
|
||||
Gyros::DataFields gyroData;
|
||||
memset(&gyroData, 0, sizeof(Gyros::DataFields));
|
||||
gyroData.x = rollRate;
|
||||
gyroData.y = pitchRate;
|
||||
gyroData.z = yawRate;
|
||||
#define Pi 3.141529654
|
||||
gyroData.x = rollRate*180/Pi;
|
||||
gyroData.y = pitchRate*180/Pi;
|
||||
gyroData.z = yawRate*180/Pi;
|
||||
gyros->setData(gyroData);
|
||||
|
||||
Accels::DataFields accelData;
|
||||
@ -420,7 +421,7 @@ void TraceBuf(const char* buf,int len)
|
||||
{
|
||||
if(i>0)
|
||||
{
|
||||
qDebug() << str;
|
||||
// qDebug() << str;
|
||||
str.clear();
|
||||
reminder=false;
|
||||
}
|
||||
@ -429,6 +430,7 @@ void TraceBuf(const char* buf,int len)
|
||||
str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0'));
|
||||
}
|
||||
|
||||
if(reminder)
|
||||
qDebug() << str;
|
||||
if(reminder){
|
||||
// qDebug() << str;
|
||||
}
|
||||
}
|
||||
|
||||
@ -44,10 +44,18 @@
|
||||
#include "utils/homelocationutil.h"
|
||||
#include "utils/worldmagmodel.h"
|
||||
|
||||
#include "../uavobjectwidgetutils/configtaskwidget.h"
|
||||
#include "extensionsystem/pluginmanager.h"
|
||||
#include "uavtalk/telemetrymanager.h"
|
||||
#include "uavobject.h"
|
||||
#include "uavobjectmanager.h"
|
||||
|
||||
#include "positionactual.h"
|
||||
#include "homelocation.h"
|
||||
#include "gpsposition.h"
|
||||
#include "gyros.h"
|
||||
#include "positionactual.h"
|
||||
#include "velocityactual.h"
|
||||
#define allow_manual_home_location_move
|
||||
|
||||
// *************************************************************************************
|
||||
@ -111,6 +119,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
|
||||
obum = pm->getObject<UAVObjectUtilManager>();
|
||||
}
|
||||
|
||||
|
||||
// **************
|
||||
// get current location
|
||||
|
||||
@ -204,6 +213,9 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
|
||||
m_map->SetCurrentPosition(m_home_position.coord); // set the map position
|
||||
m_map->Home->SetCoord(m_home_position.coord); // set the HOME position
|
||||
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
|
||||
|
||||
@ -558,19 +570,45 @@ void OPMapGadgetWidget::updatePosition()
|
||||
uav_pos = internals::PointLatLng(uav_latitude, uav_longitude);
|
||||
|
||||
// *************
|
||||
// get the current GPS details
|
||||
// get the current GPS position and heading
|
||||
GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm);
|
||||
Q_ASSERT(gpsPositionObj);
|
||||
|
||||
// get current GPS position
|
||||
if (!getGPSPosition(gps_latitude, gps_longitude, gps_altitude))
|
||||
return;
|
||||
GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData();
|
||||
|
||||
// get current GPS heading
|
||||
// gps_heading = getGPS_Heading();
|
||||
gps_heading = 0;
|
||||
gps_heading = gpsPositionData.Heading;
|
||||
gps_latitude = gpsPositionData.Latitude;
|
||||
gps_longitude = gpsPositionData.Longitude;
|
||||
gps_altitude = gpsPositionData.Altitude;
|
||||
|
||||
qDebug() << "Lon: " << gps_longitude/1e7;
|
||||
|
||||
gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
|
||||
|
||||
// *************
|
||||
//**********************
|
||||
// get the current position and heading estimates
|
||||
PositionActual *positionActualObj = PositionActual::GetInstance(obm);
|
||||
VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm);
|
||||
Gyros *gyrosObj = Gyros::GetInstance(obm);
|
||||
|
||||
Q_ASSERT(positionActualObj);
|
||||
Q_ASSERT(velocityActualObj);
|
||||
Q_ASSERT(gyrosObj);
|
||||
|
||||
PositionActual::DataFields positionActualData = positionActualObj->getData();
|
||||
VelocityActual::DataFields velocityActualData = velocityActualObj->getData();
|
||||
Gyros::DataFields gyrosData = gyrosObj->getData();
|
||||
|
||||
double NED[3]={positionActualData.North, positionActualData.East, positionActualData.Down};
|
||||
double vNED[3]={velocityActualData.North, velocityActualData.East, velocityActualData.Down};
|
||||
|
||||
//Set the position and heading estimates in the painter module
|
||||
m_map->UAV->SetNED(NED);
|
||||
m_map->UAV->SetCAS(-1); //THIS NEEDS TO BECOME AIRSPEED, ONCE WE SETTLE ON A UAVO
|
||||
m_map->UAV->SetGroundspeed(vNED);
|
||||
m_map->UAV->SetYawRate(gyrosData.z); //Not correct, but I'm being lazy right now.
|
||||
|
||||
// *************
|
||||
// display the UAV position
|
||||
|
||||
QString str =
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user