Skip to content

Q
qgroundcontrol
Commit f7cef6a2 authored by LM's avatar LM
Browse files
Options

Merged

parents f4c240a1 604d976a
Hide whitespace changes
Inline Side-by-side
Showing with 512 additions and 109 deletions
src/comm/MAVLinkProtocol.cc
View file @ f7cef6a2
......@@ -207,6 +207,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
// BIG NASTY HACK
//TODO
//BUG
//BAD
//FIXME
if (message.sysid == 35)
message.sysid = 42;
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
......
src/comm/MAVLinkSimulationLink.cc
View file @ f7cef6a2
......@@ -379,7 +379,8 @@ void MAVLinkSimulationLink::mainloop()
// Send back new setpoint
mavlink_message_t ret;
mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw);
mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI);
mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ,
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
......
src/uas/UAS.cc
View file @ f7cef6a2
......@@ -72,7 +72,7 @@ pitch(0.0),
yaw(0.0),
statusTimeout(new QTimer(this)),
paramsOnceRequested(false),
airframe(0),
airframe(QGC_AIRFRAME_EASYSTAR),
attitudeKnown(false),
paramManager(NULL),
attitudeStamped(false),
......@@ -957,7 +957,7 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
{
#ifdef MAVLINK_ENABLED_PIXHAWK
mavlink_message_t msg;
mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw);
mavlink_msg_local_position_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw/M_PI*180.0);
sendMessage(msg);
#else
Q_UNUSED(x);
......
src/uas/UASManager.cc
View file @ f7cef6a2
......@@ -96,6 +96,26 @@ bool UASManager::setHomePosition(double lat, double lon, double alt)
return changed;
}
/**
* @param x1 Point 1 coordinate in x dimension
* @param y1 Point 1 coordinate in y dimension
* @param z1 Point 1 coordinate in z dimension
*
* @param x2 Point 2 coordinate in x dimension
* @param y2 Point 2 coordinate in y dimension
* @param z2 Point 2 coordinate in z dimension
*/
void UASManager::setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2)
{
nedSafetyLimitPosition1.x() = x1;
nedSafetyLimitPosition1.y() = y1;
nedSafetyLimitPosition1.z() = z1;
nedSafetyLimitPosition2.x() = x2;
nedSafetyLimitPosition2.y() = y2;
nedSafetyLimitPosition2.z() = z2;
}
void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude)
{
......@@ -198,6 +218,7 @@ UASManager::UASManager() :
{
start(QThread::LowPriority);
loadSettings();
setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
}
UASManager::~UASManager()
......
src/uas/UASManager.h
View file @ f7cef6a2
......@@ -92,6 +92,37 @@ public:
/** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */
void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up);
void getLocalNEDSafetyLimits(double* x1, double* y1, double* z1, double* x2, double* y2, double* z2)
{
*x1 = nedSafetyLimitPosition1.x();
*y1 = nedSafetyLimitPosition1.y();
*z1 = nedSafetyLimitPosition1.z();
*x2 = nedSafetyLimitPosition2.x();
*y2 = nedSafetyLimitPosition2.y();
*z2 = nedSafetyLimitPosition2.z();
}
/** @brief Check if a position is in the local NED safety limits */
bool isInLocalNEDSafetyLimits(double x, double y, double z)
{
if (x < nedSafetyLimitPosition1.x() &&
y > nedSafetyLimitPosition1.y() &&
z < nedSafetyLimitPosition1.z() &&
x > nedSafetyLimitPosition2.x() &&
y < nedSafetyLimitPosition2.y() &&
z > nedSafetyLimitPosition2.z())
{
// Within limits
return true;
}
else
{
// Not within limits
return false;
}
}
// void wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down);
......@@ -188,6 +219,9 @@ public slots:
/** @brief Set the current home position on all UAVs*/
bool setHomePosition(double lat, double lon, double alt);
/** @brief Set the safety limits in local position frame */
void setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2);
/** @brief Update home position based on the position from one of the UAVs */
void uavChangedHomePosition(int uav, double lat, double lon, double alt);
......@@ -207,6 +241,8 @@ protected:
double homeAlt;
Eigen::Quaterniond ecef_ref_orientation_;
Eigen::Vector3d ecef_ref_point_;
Eigen::Vector3d nedSafetyLimitPosition1;
Eigen::Vector3d nedSafetyLimitPosition2;
void initReference(const double & latitude, const double & longitude, const double & altitude);
......
src/ui/HSIDisplay.cc
View file @ f7cef6a2
......@@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
bodyYawSet(0.0f),
uiXSetCoordinate(0.0f),
uiYSetCoordinate(0.0f),
uiZSetCoordinate(0.0f),
uiZSetCoordinate(-0.51f),
uiYawSet(0.0f),
metricWidth(4.0),
positionLock(false),
......@@ -100,7 +100,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
mavInitialized(false),
bottomMargin(10.0f),
topMargin(12.0f),
userSetPointSet(false)
userSetPointSet(false),
dragStarted(false),
leftDragStarted(false),
mouseHasMoved(false),
actionPending(false)
{
refreshTimer->setInterval(updateInterval);
......@@ -121,6 +125,14 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
// Do first update
setMetricWidth(metricWidth);
// Set tooltip
setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));
statusClearTimer.start(3000);
setFocusPolicy(Qt::StrongFocus);
}
void HSIDisplay::resetMAVState()
......@@ -200,8 +212,9 @@ void HSIDisplay::renderOverlay()
painter.setPen(pen);
const int ringCount = 2;
for (int i = 0; i < ringCount; i++) {
float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.3f * i+1) / 2.0f - bottomMargin / 2.0f;
float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), QGC::colorCyan, 1.6f, vwidth/2-4, vheight/2+radius+2.2, &painter);
}
// Draw orientation labels
......@@ -217,6 +230,15 @@ void HSIDisplay::renderOverlay()
painter.rotate(+yawRotate);
painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
// Draw trail
// QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// // Transform from body to world coordinates
// m = metricWorldToBody(m);
// // Scale from metric body to screen reference units
// QPointF s = metricBodyToRef(m);
// drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
// Draw center indicator
// QPolygonF p(3);
// p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
......@@ -252,28 +274,41 @@ void HSIDisplay::renderOverlay()
// Draw position setpoints in body coordinates
if (userSetPointSet) {
float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate;
float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate;
float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate;
float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff);
float angleDiff = uiYawSet - bodyYawSet;
float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff)));
if (userSetPointSet && setPointDist > 0.08f || normAngleDiff > 0.05f || dragStarted)
{
QColor spColor(150, 150, 150);
drawSetpointXY(uiXSetCoordinate, uiYSetCoordinate, uiYawSet, spColor, painter);
drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
}
if (positionSetPointKnown) {
// Labels on outer part and bottom
// Draw waypoints
drawWaypoints(painter);
// Draw setpoint over waypoints
if (positionSetPointKnown)
{
// Draw setpoint
drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::colorCyan, painter);
drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, uas->getColor(), painter);
// Draw travel direction line
QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// Transform from body to world coordinates
m = metricWorldToBody(m);
// Scale from metric body to screen reference units
QPointF s = metricBodyToRef(m);
drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorCyan, &painter);
drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
}
// Labels on outer part and bottom
// Draw waypoints
drawWaypoints(painter);
// Draw status flags
drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, attControlKnown, painter);
drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, xyControlKnown, painter);
......@@ -300,20 +335,25 @@ void HSIDisplay::renderOverlay()
// Position
QString str;
str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter);
paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
}
if (globalAvailable > 0) {
// Position
QString str;
str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter);
paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
}
// Draw Field of view to bottom right
paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter);
// Draw Safety
double x1, y1, z1, x2, y2, z2;
UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
// drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
// Draw status message
paintText(statusMessage, QGC::colorOrange, 2.4f, 8, 15, &painter);
}
void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
......@@ -333,7 +373,8 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bo
painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight));
paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter);
// Cross out instrument if state unknown
if (!known) {
if (!known)
{
QPen pen(Qt::yellow);
pen.setWidth(2);
painter.setPen(pen);
......@@ -466,13 +507,13 @@ QPointF HSIDisplay::metricBodyToWorld(QPointF body)
{
// First rotate into world coordinates
// then translate to world position
QPointF result((cos(yaw) * body.x()) + (sin(yaw) * body.y()) + x, (-sin(yaw) * body.x()) + (cos(yaw) * body.y()) + y);
QPointF result((cos(-yaw) * body.x()) + (sin(-yaw) * body.y()) + x, (-sin(-yaw) * body.x()) + (cos(-yaw) * body.y()) + y);
return result;
}
QPointF HSIDisplay::screenToMetricBody(QPointF ref)
{
return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth - x, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth - y);
return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
}
QPointF HSIDisplay::refToMetricBody(QPointF &ref)
......@@ -488,6 +529,16 @@ QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}
double HSIDisplay::metricToRef(double metric)
{
return (metric / metricWidth) * vwidth;
}
double HSIDisplay::refToMetric(double ref)
{
return (ref/vwidth) * metricWidth;
}
QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
{
QPointF ref = metricBodyToRef(metric);
......@@ -498,30 +549,93 @@ QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
static bool dragStarted;
static float startX;
if (event->MouseButtonDblClick) {
//setBodySetpointCoordinateXY(-refToMetric(screenToRefY(event->y()) - yCenterPos), refToMetric(screenToRefX(event->x()) - xCenterPos));
if (event->type() == QMouseEvent::MouseButtonDblClick)
{
QPointF p = screenToMetricBody(event->posF());
setBodySetpointCoordinateXY(p.x(), p.y());
qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
} else if (event->MouseButtonPress) {
startX = event->globalX();
if (event->button() == Qt::RightButton) {
// qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
}
}
void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
{
if (mouseHasMoved)
{
if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
{
if (dragStarted)
{
setBodySetpointCoordinateYaw(uiYawSet);
dragStarted = false;
}
}
if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton)
{
if (leftDragStarted)
{
setBodySetpointCoordinateZ(uiZSetCoordinate);
leftDragStarted = false;
}
}
}
mouseHasMoved = false;
}
void HSIDisplay::mousePressEvent(QMouseEvent * event)
{
if (event->type() == QMouseEvent::MouseButtonPress)
{
if (event->button() == Qt::RightButton)
{
startX = event->x();
// Start tracking mouse move
dragStarted = true;
} else if (event->button() == Qt::LeftButton) {
}
else if (event->button() == Qt::LeftButton)
{
startY = event->y();
leftDragStarted = true;
}
}
mouseHasMoved = false;
}
void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
{
if (event->type() == QMouseEvent::MouseMove)
{
if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width();
if (leftDragStarted)
{
// uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
// setBodySetpointCoordinateZ(uiZSetCoordinate);
}
} else if (event->MouseButtonRelease) {
dragStarted = false;
} else if (event->MouseMove) {
if (dragStarted) uiYawSet += (startX - event->globalX()) / this->frameSize().width();
if (leftDragStarted || dragStarted) mouseHasMoved = true;
}
}
void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
{
actionPending = false;
statusMessage = "SETPOINT SENT";
statusClearTimer.start();
sendBodySetPointCoordinates();
}
else
{
HDDisplay::keyPressEvent(event);
}
}
void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
{
event->ignore();
}
void HSIDisplay::setMetricWidth(double width)
{
if (width != metricWidth) {
......@@ -593,6 +707,8 @@ void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz,
void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{
userSetPointSet = true;
userXYSetPointSet = true;
// Set coordinates and send them out to MAV
QPointF sp(x, y);
......@@ -602,21 +718,66 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
if (uas && mavInitialized) {
uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y;
if (uas && mavInitialized)
{
//sendBodySetPointCoordinates();
statusMessage = "POSITION SET, PRESS <ENTER> TO SEND";
actionPending = true;
statusClearTimer.start();
}
}
void HSIDisplay::setBodySetpointCoordinateZ(double z)
{
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiZSetCoordinate = z;
statusMessage = "Z SET, PRESS <ENTER> TO SEND";
actionPending = true;
statusClearTimer.start();
//sendBodySetPointCoordinates();
}
void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
if (!userXYSetPointSet && setPointKnown)
{
uiXSetCoordinate = bodyXSetCoordinate;
uiYSetCoordinate = bodyYSetCoordinate;
}
else if (!userXYSetPointSet && mavInitialized)
{
QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
uiXSetCoordinate = coord.x();
uiYSetCoordinate = coord.y();
}
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw), cos(yaw));
statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
statusClearTimer.start();
actionPending = true;
//sendBodySetPointCoordinates();
}
void HSIDisplay::sendBodySetPointCoordinates()
{
// Send the coordinates to the MAV
if (uas && mavInitialized)
{
double dx = uiXSetCoordinate - uas->getLocalX();
double dy = uiYSetCoordinate - uas->getLocalY();
double dz = uiZSetCoordinate - uas->getLocalZ();
bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 1.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate);
if (valid)
{
uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
}
else
{
setStatusMessage("REJECTED NEW SETPOINT: OUT OF BOUNDS");
}
}
}
void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
......@@ -650,11 +811,13 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
setPointKnown = true;
positionSetPointKnown = true;
// qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
// posXSet = xDesired;
// posYSet = yDesired;
// posZSet = zDesired;
// posYawSet = yawDesired;
if (!userSetPointSet && !dragStarted)
{
uiXSetCoordinate = bodyXSetCoordinate;
uiYSetCoordinate = bodyYSetCoordinate;
// uiZSetCoordinate = bodyZSetCoordinate;
uiYawSet= bodyYawSet;
}
}
void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
......@@ -748,10 +911,10 @@ QColor HSIDisplay::getColorForSNR(float snr)
return color;
}
void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter)
void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter)
{
if (setPointKnown) {
float radius = vwidth / 20.0f;
if (setPointKnown && uas) {
float radius = vwidth / 18.0f;
QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.4f));
pen.setColor(color);
......@@ -762,9 +925,25 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
in = metricWorldToBody(in);
// Scale from metric to screen reference coordinates
QPointF p = metricBodyToRef(in);
drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
QPolygonF poly(4);
// Top point
poly.replace(0, QPointF(p.x()-radius, p.y()-radius));
// Right point
poly.replace(1, QPointF(p.x()+radius, p.y()-radius));
// Bottom point
poly.replace(2, QPointF(p.x()+radius, p.y()+radius));
poly.replace(3, QPointF(p.x()-radius, p.y()+radius));
// Label
paintText(QString("z: %1 m").arg(z), color, 1.2f, p.x()-radius, p.y()-radius-2.0f, &painter);
drawPolygon(poly, &painter);
radius *= 0.8f;
drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.4f), color, &painter);
// Draw center dot
painter.setBrush(color);
drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
}
......@@ -772,20 +951,9 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
void HSIDisplay::drawWaypoints(QPainter& painter)
{
if (uas) {
if (uas)
{
const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointList();
// for (int i = 0; i < list.size(); i++)
// {
// QPointF in(list.at(i)->getX(), list.at(i)->getY());
// // Transform from world to body coordinates
// in = metricWorldToBody(in);
// // Scale from metric to screen reference coordinates
// QPointF p = metricBodyToRef(in);
// Waypoint2DIcon* wp = new Waypoint2DIcon();
// wp->setLocalPosition(list.at(i)->getX(), list.at(i)->getY());
// wp->setPos(0, 0);
// scene()->addItem(wp);
// }
QColor color;
painter.setBrush(Qt::NoBrush);
......@@ -814,7 +982,6 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
painter.setBrush(Qt::NoBrush);
// DRAW WAYPOINT
//drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
float waypointSize = vwidth / 20.0f * 2.0f;
QPolygonF poly(4);
// Top point
......@@ -827,25 +994,28 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
// Select color based on if this is the current waypoint
if (list.at(i)->getCurrent()) {
color = QGC::colorCyan;//uas->getColor();
color = QGC::colorYellow;//uas->getColor();
pen.setWidthF(refLineWidthToPen(0.8f));
} else {
color = uas->getColor();
color = QGC::colorCyan;
pen.setWidthF(refLineWidthToPen(0.4f));
}
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()+yaw) * radius, p.y()-cos(list.at(i)->getYaw()+yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
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()) {
if (!lastWaypoint.isNull())
{
pen.setWidthF(refLineWidthToPen(0.4f));
painter.setPen(pen);
color = uas->getColor();
color = QGC::colorCyan;
drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
}
lastWaypoint = p;
......@@ -858,6 +1028,7 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig
QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.1f));
pen.setColor(color);
pen.setBrush(Qt::NoBrush);
painter.setPen(pen);
painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
}
......@@ -1026,7 +1197,7 @@ void HSIDisplay::wheelEvent(QWheelEvent* event)
// Increase width -> Zoom out
metricWidth -= event->delta() * zoomScale;
}
metricWidth = qBound(0.1, metricWidth, 9999.0);
metricWidth = qBound(0.5, metricWidth, 9999.0);
emit metricWidthChanged(metricWidth);
}
......
src/ui/HSIDisplay.h
View file @ f7cef6a2
......@@ -85,6 +85,14 @@ public slots:
void pressKey(int key);
/** @brief Reset the state of the view */
void resetMAVState();
/** @brief Clear the status message */
void clearStatusMessage()
{
statusMessage = "";
if (actionPending) statusMessage = "TIMED OUT, NO ACTION";
statusClearTimer.start();
actionPending = false;
}
signals:
void metricWidthChanged(double width);
......@@ -101,19 +109,33 @@ protected slots:
/** @brief Draw a position lock indicator */
void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter);
void setBodySetpointCoordinateXY(double x, double y);
void setBodySetpointCoordinateYaw(double yaw);
void setBodySetpointCoordinateZ(double z);
/** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */
void sendBodySetPointCoordinates();
/** @brief Draw one setpoint */
void drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter);
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 the limiting safety area */
void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter);
/** @brief Receive mouse clicks */
void mouseDoubleClickEvent(QMouseEvent* event);
void mousePressEvent(QMouseEvent * event);
void mouseReleaseEvent(QMouseEvent * event);
void mouseMoveEvent(QMouseEvent * event);
/** @brief Receive mouse wheel events */
void wheelEvent(QWheelEvent* event);
/** @brief Read out send keys */
void keyPressEvent(QKeyEvent* event);
/** @brief Ignore context menu event */
void contextMenuEvent (QContextMenuEvent* event);
/** @brief Set status message on screen */
void setStatusMessage(const QString& message)
{
statusMessage = message;
statusClearTimer.start();
}
protected:
......@@ -131,6 +153,10 @@ protected:
QPointF refToMetricBody(QPointF &ref);
/** @brief Metric coordinates to reference coordinates */
QPointF metricBodyToRef(QPointF &metric);
/** @brief Metric length to reference coordinates */
double metricToRef(double metric);
/** @bried Reference coordinates to metric length */
double refToMetric(double ref);
/** @brief Metric body coordinates to screen coordinates */
QPointF metricBodyToScreen(QPointF metric);
QMap<int, QString> objectNames;
......@@ -138,6 +164,14 @@ protected:
QMap<int, float> objectQualities;
QMap<int, float> objectBearings;
QMap<int, float> objectDistances;
bool dragStarted;
bool leftDragStarted;
bool mouseHasMoved;
float startX;
float startY;
QTimer statusClearTimer;
QString statusMessage;
bool actionPending;
/**
* @brief Private data container class to be used within the HSI widget
......@@ -254,7 +288,8 @@ protected:
// Data indicators
bool setPointKnown; ///< Controller setpoint known status flag
bool positionSetPointKnown; ///< Position setpoint known status flag
bool userSetPointSet;
bool userSetPointSet; ///< User set X, Y and Z
bool userXYSetPointSet; ///< User set the X/Y position already
private:
};
......
src/ui/Linechart.ui
View file @ f7cef6a2
......@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>662</width>
<height>381</height>
<width>833</width>
<height>585</height>
</rect>
</property>
<property name="sizePolicy">
......@@ -25,23 +25,8 @@
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2" stretch="10,20">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>2</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>2</number>
</property>
<property name="bottomMargin">
<number>2</number>
</property>
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="5,5,5,5,100">
<item row="0" column="0" colspan="4">
<widget class="QGroupBox" name="curveGroupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="MinimumExpanding">
......@@ -114,8 +99,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>210</width>
<height>361</height>
<width>232</width>
<height>516</height>
</rect>
</property>
</widget>
......@@ -124,7 +109,7 @@
</layout>
</widget>
</item>
<item>
<item row="0" column="4" rowspan="2">
<widget class="QGroupBox" name="diagramGroupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
......@@ -149,6 +134,33 @@
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="shortNameCheckBox">
<property name="text">
<string>Short Names</string>
</property>
</widget>
</item>
<item row="1" column="2" colspan="2">
<widget class="QPushButton" name="recolorButton">
<property name="text">
<string>Recolor</string>
</property>
</widget>
</item>
<item row="1" column="1">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<resources/>
......
src/ui/QGCToolBar.cc
View file @ f7cef6a2
......@@ -64,7 +64,7 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
toolBarBatteryBar->setMinimum(0);
toolBarBatteryBar->setMaximum(100);
toolBarBatteryBar->setMinimumWidth(20);
toolBarBatteryBar->setMaximumWidth(200);
toolBarBatteryBar->setMaximumWidth(100);
toolBarBatteryVoltageLabel = new QLabel("xx.x V");
toolBarBatteryVoltageLabel->setStyleSheet(QString("QLabel { margin: 0px 0px 0px 4px; font: 14px; color: %1; }").arg(QColor(Qt::green).name()));
symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 0px 0px 20px; background-color: none; }");
......
src/ui/WaypointList.cc
View file @ f7cef6a2
......@@ -210,7 +210,7 @@ void WaypointList::addCurrentPositonWaypoint()
yawGlobal = last->getYaw();
}
// Create global frame waypoint per default
wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
uas->getWaypointManager()->addWaypoint(wp);
updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
}
......
src/ui/designer/QGCCommandButton.cc
View file @ f7cef6a2
......@@ -53,6 +53,7 @@ QGCCommandButton::QGCCommandButton(QWidget *parent) :
ui->editCommandComboBox->addItem("CUSTOM 13", 13);
ui->editCommandComboBox->addItem("CUSTOM 14", 14);
ui->editCommandComboBox->addItem("CUSTOM 15", 15);
ui->editCommandComboBox->addItem("NAV_WAYPOINT", 16);
ui->editCommandComboBox->setEditable(true);
}
......@@ -75,6 +76,7 @@ void QGCCommandButton::sendCommand()
int component = ui->editComponentSpinBox->value();
QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
} else {
qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
}
......
src/ui/linechart/LinechartPlot.cc
View file @ f7cef6a2
......@@ -127,6 +127,7 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt
//updateTimer->start(DEFAULT_REFRESH_RATE);
connect(&timeoutTimer, SIGNAL(timeout()), this, SLOT(removeTimedOutCurves()));
//timeoutTimer.start(5000);
}
LinechartPlot::~LinechartPlot()
......@@ -208,7 +209,7 @@ void LinechartPlot::removeTimedOutCurves()
foreach(QString key, lastUpdate.keys())
{
quint64 time = lastUpdate.value(key);
if (QGC::groundTimeMilliseconds() - time > 20000)
if (QGC::groundTimeMilliseconds() - time > 10000)
{
// Remove this curve
// Delete curves
......@@ -227,6 +228,7 @@ void LinechartPlot::removeTimedOutCurves()
delete d;
// Set the pointer null
d = NULL;
emit curveRemoved(key);
}
}
}
......
src/ui/linechart/LinechartPlot.h
View file @ f7cef6a2
......@@ -269,6 +269,17 @@ public slots:
void setAverageWindow(int windowSize);
void removeTimedOutCurves();
/** @brief Reset color map */
void shuffleColors()
{
foreach (QwtPlotCurve* curve, curves)
{
QPen pen(curve->pen());
pen.setColor(getNextColor());
curve->setPen(pen);
}
}
public:
QColor getColorForCurve(QString id);
......
src/ui/linechart/LinechartWidget.cc
View file @ f7cef6a2
......@@ -76,7 +76,7 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
{
// Add elements defined in Qt Designer
ui.setupUi(this);
this->setMinimumSize(300, 200);
this->setMinimumSize(200, 150);
// Add and customize curve list elements (left side)
curvesWidget = new QWidget(ui.curveListWidget);
......@@ -104,7 +104,8 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
QLabel* mean;
QLabel* variance;
//horizontalLayout->addWidget(checkBox);
connect(ui.recolorButton, SIGNAL(clicked()), this, SLOT(recolor()));
connect(ui.shortNameCheckBox, SIGNAL(clicked(bool)), this, SLOT(setShortNames(bool)));
int labelRow = curvesWidgetLayout->rowCount();
......@@ -170,6 +171,7 @@ void LinechartWidget::writeSettings()
settings.beginGroup("LINECHART");
if (timeButton) settings.setValue("ENFORCE_GROUNDTIME", timeButton->isChecked());
if (unitsCheckBox) settings.setValue("SHOW_UNITS", unitsCheckBox->isChecked());
if (ui.shortNameCheckBox) settings.setValue("SHORT_NAMES", ui.shortNameCheckBox->isChecked());
settings.endGroup();
settings.sync();
}
......@@ -183,7 +185,8 @@ void LinechartWidget::readSettings()
timeButton->setChecked(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool());
activePlot->enforceGroundTime(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool());
}
if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS").toBool());
if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS", unitsCheckBox->isChecked()).toBool());
if (ui.shortNameCheckBox) ui.shortNameCheckBox->setChecked(settings.value("SHORT_NAMES", ui.shortNameCheckBox->isChecked()).toBool());
settings.endGroup();
}
......@@ -280,7 +283,8 @@ void LinechartWidget::createLayout()
connect(this, SIGNAL(curveRemoved(QString)), activePlot, SLOT(hideCurve(QString)));
// Update scrollbar when plot window changes (via translator method setPlotWindowPosition()
connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64)));
// connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64)));
connect(activePlot, SIGNAL(curveRemoved(QString)), this, SLOT(removeCurve(QString)));
// Update plot when scrollbar is moved (via translator method setPlotWindowPosition()
connect(this, SIGNAL(plotWindowPositionUpdated(quint64)), activePlot, SLOT(setWindowPosition(quint64)));
......@@ -614,6 +618,8 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
QLabel* mean;
QLabel* variance;
curveNames.insert(curve+unit, curve);
int labelRow = curvesWidgetLayout->rowCount();
checkBox = new QCheckBox(this);
......@@ -642,6 +648,9 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
colorIcon->setStyleSheet(colorstyle);
colorIcon->setAutoFillBackground(true);
// Label
curveNameLabels.insert(curve+unit, label);
// Value
value = new QLabel(this);
value->setNum(0.00);
......@@ -720,8 +729,96 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
void LinechartWidget::removeCurve(QString curve)
{
Q_UNUSED(curve)
//TODO @todo Ensure that the button for a curve gets deleted when the original curve is deleted
// Remove name
QWidget* widget = NULL;
widget = curveLabels->take(curve);
curvesWidgetLayout->removeWidget(widget);
widget->deleteLater();
widget = curveMeans->take(curve);
curvesWidgetLayout->removeWidget(widget);
widget->deleteLater();
widget = curveMedians->take(curve);
curvesWidgetLayout->removeWidget(widget);
widget->deleteLater();
widget = curveVariances->take(curve);
curvesWidgetLayout->removeWidget(widget);
widget->deleteLater();
// widget = colorIcons->take(curve);
// curvesWidgetLayout->removeWidget(colorIcons->take(curve));
widget->deleteLater();
// intData->remove(curve);
}
void LinechartWidget::recolor()
{
activePlot->shuffleColors();
foreach (QString key, colorIcons.keys())
{
// FIXME
// if (activePlot)
QString colorstyle;
QColor color = activePlot->getColorForCurve(key);
colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue());
QWidget* colorIcon = colorIcons.value(key, 0);
if (colorIcon)
{
colorIcon->setStyleSheet(colorstyle);
colorIcon->setAutoFillBackground(true);
}
}
}
void LinechartWidget::setShortNames(bool enable)
{
foreach (QString key, curveNames.keys())
{
QString name;
if (enable)
{
QStringList parts = curveNames.value(key).split(".");
if (parts.length() > 1)
{
name = parts.at(1);
}
else
{
name = parts.at(0);
}
const unsigned int sizeLimit = 10;
// Replace known words with abbreviations
if (name.length() > sizeLimit)
{
name.replace("gyroscope", "gyro");
name.replace("accelerometer", "acc");
name.replace("magnetometer", "mag");
name.replace("distance", "dist");
name.replace("altitude", "alt");
name.replace("waypoint", "wp");
name.replace("error", "err");
name.replace("message", "msg");
name.replace("source", "src");
}
// Check if sub-part is still exceeding N chars
if (name.length() > sizeLimit)
{
name.replace("a", "");
name.replace("e", "");
name.replace("i", "");
name.replace("o", "");
name.replace("u", "");
}
}
else
{
name = curveNames.value(key);
}
curveNameLabels.value(key)->setText(name);
}
}
void LinechartWidget::showEvent(QShowEvent* event)
......
src/ui/linechart/LinechartWidget.h
View file @ f7cef6a2
......@@ -73,6 +73,10 @@ public:
public slots:
void addCurve(const QString& curve, const QString& unit);
void removeCurve(QString curve);
/** @brief Recolor all curves */
void recolor();
/** @brief Set short names for curves */
void setShortNames(bool enable);
/** @brief Append data without unit */
void appendData(int uasId, QString curve, double data, quint64 usec);
/** @brief Append data with unit */
......@@ -130,6 +134,8 @@ protected:
int curveListCounter; ///< Counter of curves in curve list
QList<QString>* listedCurves; ///< Curves listed
QMap<QString, QLabel*>* curveLabels; ///< References to the curve labels
QMap<QString, QLabel*> curveNameLabels; ///< References to the curve labels
QMap<QString, QString> curveNames; ///< Full curve names
QMap<QString, QLabel*>* curveMeans; ///< References to the curve means
QMap<QString, QLabel*>* curveMedians; ///< References to the curve medians
QMap<QString, QLabel*>* curveVariances; ///< References to the curve variances
......
src/ui/uas/UASListWidget.cc
View file @ f7cef6a2
......@@ -114,8 +114,8 @@ void UASListWidget::activeUAS(UASInterface* uas)
void UASListWidget::removeUAS(UASInterface* uas)
{
uasViews.remove(uas);
listLayout->removeWidget(uasViews.value(uas));
uasViews.value(uas)->deleteLater();
// uasViews.remove(uas);
// listLayout->removeWidget(uasViews.value(uas));
// uasViews.value(uas)->deleteLater();
}
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