Skip to content

Q
qgroundcontrol
Commit 8e10e09e authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Improvements across application

parent 30cff0cd
Hide whitespace changes
Inline Side-by-side
Showing with 80 additions and 39 deletions
src/QGC.cc
View file @ 8e10e09e
......@@ -22,7 +22,6 @@ This file is part of the QGROUNDCONTROL project
======================================================================*/
#include "QGC.h"
#include <qmath.h>
#include <float.h>
......
src/QGC.h
View file @ 8e10e09e
......@@ -69,7 +69,7 @@ const QColor colorRed(154, 20, 20);
const QColor colorGreen(20, 200, 20);
const QColor colorYellow(255, 255, 0);
const QColor colorOrange(255, 140, 0);
const QColor colorMagenta(255, 0, 65);
const QColor colorMagenta(255, 0, 55);
const QColor colorDarkWhite(240, 240, 240);
const QColor colorDarkYellow(180, 180, 0);
const QColor colorBackground("#050508");
......
src/uas/UAS.cc
View file @ 8e10e09e
......@@ -909,13 +909,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(&message, &wpc);
if (wpc.target_system == mavlink->getSystemId())
if(wpc.target_system == mavlink->getSystemId() || wpc.target_system == 0)
{
waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
}
else
{
qDebug() << "Got waypoint message, but was not for me";
qDebug() << "Got waypoint message, but was wrong system id" << wpc.target_system;
}
}
break;
......@@ -925,13 +925,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(&message, &wp);
//qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
if(wp.target_system == mavlink->getSystemId())
if(wp.target_system == mavlink->getSystemId() || wp.target_system == 0)
{
waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
}
else
{
qDebug() << "Got waypoint message, but was not for me";
qDebug() << "Got waypoint message, but was wrong system id" << wp.target_system;
}
}
break;
......@@ -940,7 +940,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(&message, &wpa);
if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
if((wpa.target_system == mavlink->getSystemId() || wpa.target_system == 0) &&
(wpa.target_component == mavlink->getComponentId() || wpa.target_component == 0))
{
waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
}
......@@ -951,13 +952,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(&message, &wpr);
if(wpr.target_system == mavlink->getSystemId())
if(wpr.target_system == mavlink->getSystemId() || wpr.target_system == 0)
{
waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
}
else
{
qDebug() << "Got waypoint message, but was not for me";
qDebug() << "Got waypoint message, but was wrong system id" << wpr.target_system;
}
}
break;
......
src/ui/HSIDisplay.cc
View file @ 8e10e09e
......@@ -1116,18 +1116,67 @@ void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const
}
}
void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& list, int i, const QPointF& p)
{
painter.setBrush(Qt::NoBrush);
// Setup pen for background
QPen penBg(color);
penBg.setWidthF(width*2.0f);
// Setup pen for foreground
QPen pen(color);
pen.setWidthF(width);
// DRAW WAYPOINT
float waypointSize = vwidth / 20.0f * 2.0f;
QPolygonF poly(4);
// Top point
poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
// Right point
poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
// Bottom point
poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
float acceptRadius = list.at(i)->getAcceptanceRadius();
// Draw background
pen.setColor(Qt::black);
painter.setPen(penBg);
drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
drawPolygon(poly, &painter);
drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::black, &painter);
// Draw foreground
pen.setColor(color);
painter.setPen(pen);
drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
drawPolygon(poly, &painter);
drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
}
void HSIDisplay::drawWaypoints(QPainter& painter)
{
if (uas)
{
const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();
// Do not work on empty lists
if (list.size() == 0) return;
QColor color;
painter.setBrush(Qt::NoBrush);
// XXX Ugly hacks, needs rewrite
QPointF lastWaypoint;
QPointF currentWaypoint;
int currentIndex = 0;
for (int i = 0; i < list.size(); i++) {
for (int i = 0; i < list.size(); i++)
{
QPointF in;
if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED)
{
......@@ -1144,42 +1193,22 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
// Scale from metric to screen reference coordinates
QPointF p = metricBodyToRef(in);
// Setup pen
QPen pen(color);
painter.setBrush(Qt::NoBrush);
// DRAW WAYPOINT
float waypointSize = vwidth / 20.0f * 2.0f;
QPolygonF poly(4);
// Top point
poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
// Right point
poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
// Bottom point
poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
// Select color based on if this is the current waypoint
if (list.at(i)->getCurrent()) {
color = QGC::colorYellow;//uas->getColor();
pen.setWidthF(refLineWidthToPen(0.8f));
} else {
color = QGC::colorCyan;
pen.setWidthF(refLineWidthToPen(0.4f));
if (list.at(i)->getCurrent())
{
currentIndex = i;
currentWaypoint = p;
}
else
{
drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), list, i, p);
}
pen.setColor(color);
painter.setPen(pen);
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
drawPolygon(poly, &painter);
float acceptRadius = list.at(i)->getAcceptanceRadius();
drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
// DRAW CONNECTING LINE
// Draw line from last waypoint to this one
if (!lastWaypoint.isNull())
{
QPen pen(QGC::colorCyan);
pen.setWidthF(refLineWidthToPen(0.4f));
painter.setPen(pen);
color = QGC::colorCyan;
......@@ -1187,6 +1216,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
}
lastWaypoint = p;
}
drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), list, currentIndex, currentWaypoint);
}
}
......
src/ui/HSIDisplay.h
View file @ 8e10e09e
......@@ -120,6 +120,8 @@ protected slots:
void drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter);
/** @brief Draw waypoints of this system */
void drawWaypoints(QPainter& painter);
/** @brief Draw one waypoint */
void drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& list, int i, const QPointF& p);
/** @brief Draw the limiting safety area */
void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter);
/** @brief Receive mouse clicks */
......
src/ui/MAVLinkDecoder.cc
View file @ 8e10e09e
......@@ -41,6 +41,7 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false);
textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false);
textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false);
// textMessageFilter.insert(MAVLINK_MSG_ID_HIGHRES_IMU, false);
connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
}
......@@ -239,6 +240,13 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
name = name.arg(debug.name).arg(fieldName);
time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
}
// else if (msgid == MAVLINK_MSG_ID_HIGHRES_IMU)
// {
// mavlink_highres_imu_t d;
// mavlink_msg_highres_imu_decode(msg, &d);
// name = name.arg(debug.name).arg(fieldName);
// time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms);
// }
else
{
name = name.arg(messageInfo[msgid].name, fieldName);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment