1
0
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 2012-08-02 10:58:55 +02:00
parent 1d42b64965
commit 3b9e3c0054
4 changed files with 215 additions and 23 deletions

View File

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

View File

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

View File

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

View File

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