Commit fea454e8 authored by pixhawk's avatar pixhawk

Added support for more generic waypoints / mission items

parent 86eba06b
......@@ -155,7 +155,8 @@ FORMS += src/ui/MainWindow.ui \
src/ui/designer/QGCParamSlider.ui \
src/ui/designer/QGCActionButton.ui \
src/ui/QGCMAVLinkLogPlayer.ui \
src/ui/QGCWaypointListMulti.ui
src/ui/QGCWaypointListMulti.ui \
src/ui/mission/QGCCustomWaypointAction.ui
INCLUDEPATH += src \
src/ui \
......
......@@ -42,8 +42,10 @@ Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bo
action(_action),
autocontinue(_autocontinue),
current(_current),
orbit(_orbit),
holdTime(_holdTime),
orbit(0),
orbitDirection(0),
param1(_orbit),
param2(_holdTime),
name(QString("WP%1").arg(id, 2, 10, QChar('0')))
{
}
......@@ -60,7 +62,7 @@ void Waypoint::save(QTextStream &saveStream)
position = position.arg(y, 0, 'g', 18);
position = position.arg(z, 0, 'g', 18);
position = position.arg(yaw, 0, 'g', 8);
saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position << "\t" << this->getAutoContinue() << "\r\n";
saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << orbit << "\t" << orbitDirection << "\t" << param1 << "\t" << param2 << "\t" << this->getCurrent() << "\t" << position << "\t" << this->getAutoContinue() << "\r\n";
}
bool Waypoint::load(QTextStream &loadStream)
......@@ -72,9 +74,9 @@ bool Waypoint::load(QTextStream &loadStream)
this->frame = (MAV_FRAME) wpParams[1].toInt();
this->action = (MAV_ACTION) wpParams[2].toInt();
this->orbit = wpParams[3].toDouble();
//TODO: orbit direction
//TODO: param1
this->holdTime = wpParams[6].toInt();
this->orbitDirection = wpParams[4].toInt();
this->param1 = wpParams[5].toDouble();
this->param2 = wpParams[6].toDouble();
this->current = (wpParams[7].toInt() == 1 ? true : false);
this->x = wpParams[8].toDouble();
this->y = wpParams[9].toDouble();
......@@ -130,6 +132,15 @@ void Waypoint::setYaw(double yaw)
}
}
void Waypoint::setAction(int action)
{
if (this->action != (MAV_ACTION)action)
{
this->action = (MAV_ACTION)action;
emit changed(this);
}
}
void Waypoint::setAction(MAV_ACTION action)
{
if (this->action != action)
......@@ -166,7 +177,52 @@ void Waypoint::setCurrent(bool current)
}
}
void Waypoint::setOrbit(double orbit)
void Waypoint::setAcceptanceRadius(double radius)
{
if (this->param1 != radius)
{
this->param1 = radius;
emit changed(this);
}
}
void Waypoint::setParam1(double param1)
{
if (this->param1 != param1)
{
this->param1 = param1;
emit changed(this);
}
}
void Waypoint::setParam2(double param2)
{
if (this->param2 != param2)
{
this->param2 = param2;
emit changed(this);
}
}
void Waypoint::setParam3(double param3)
{
if (this->x != param3)
{
this->x = param3;
emit changed(this);
}
}
void Waypoint::setParam4(double param4)
{
if (this->y != param4)
{
this->y = param4;
emit changed(this);
}
}
void Waypoint::setLoiterOrbit(double orbit)
{
if (this->orbit != orbit)
{
......@@ -177,9 +233,18 @@ void Waypoint::setOrbit(double orbit)
void Waypoint::setHoldTime(int holdTime)
{
if (this->holdTime != holdTime)
if (this->param2 != holdTime)
{
this->param2 = holdTime;
emit changed(this);
}
}
void Waypoint::setTurns(int turns)
{
if (this->param1 != turns)
{
this->holdTime = holdTime;
this->param1 = turns;
emit changed(this);
}
}
......
......@@ -55,8 +55,14 @@ public:
double getYaw() const { return yaw; }
bool getAutoContinue() const { return autocontinue; }
bool getCurrent() const { return current; }
double getOrbit() const { return orbit; }
double getHoldTime() const { return holdTime; }
double getLoiterOrbit() const { return orbit; }
double getAcceptanceRadius() const { return param1; }
double getHoldTime() const { return param2; }
double getParam1() const { return param1; }
double getParam2() const { return param2; }
double getParam3() const { return x; }
double getParam4() const { return y; }
int getTurns() const { return param1; }
MAV_FRAME getFrame() const { return frame; }
MAV_ACTION getAction() const { return action; }
const QString& getName() const { return name; }
......@@ -76,7 +82,10 @@ protected:
bool autocontinue;
bool current;
double orbit;
int holdTime;
int orbitDirection;
double param1;
double param2;
int turns;
QString name;
public slots:
......@@ -85,20 +94,21 @@ public slots:
void setY(double y);
void setZ(double z);
void setYaw(double yaw);
/** @brief Set the waypoint action */
void setAction(int action);
void setAction(MAV_ACTION action);
void setFrame(MAV_FRAME frame);
void setAutocontinue(bool autoContinue);
void setCurrent(bool current);
void setOrbit(double orbit);
void setLoiterOrbit(double orbit);
void setParam1(double param1);
void setParam2(double param2);
void setParam3(double param3);
void setParam4(double param4);
void setAcceptanceRadius(double radius);
void setHoldTime(int holdTime);
// //for QDoubleSpin
// void setX(double x);
// void setY(double y);
// void setZ(double z);
// void setYaw(double yaw);
// void setOrbit(double orbit);
/** @brief Number of turns for loiter waypoints */
void setTurns(int turns);
signals:
/** @brief Announces a change to the waypoint data */
......
......@@ -265,7 +265,7 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
switch (action.action)
{
case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF:
flying = true;
break;
default:
......
......@@ -1480,7 +1480,7 @@ void UAS::launch()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH);
mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
......@@ -1494,7 +1494,7 @@ void UAS::enable_motors()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
......@@ -1508,7 +1508,7 @@ void UAS::disable_motors()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
......
......@@ -555,10 +555,10 @@ void UASWaypointManager::writeWaypoints()
cur_d->autocontinue = cur_s->getAutoContinue();
cur_d->current = cur_s->getCurrent() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen
cur_d->orbit = 0;
cur_d->orbit = cur_s->getLoiterOrbit();
cur_d->orbit_direction = 0;
cur_d->param1 = cur_s->getOrbit();
cur_d->param2 = cur_s->getHoldTime();
cur_d->param1 = cur_s->getParam1();
cur_d->param2 = cur_s->getParam2();
cur_d->frame = cur_s->getFrame();
cur_d->action = cur_s->getAction();
cur_d->seq = i; // don't read out the sequence number of the waypoint class
......
......@@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project
#include "Waypoint.h"
#include "UASWaypointManager.h"
#include "Waypoint2DIcon.h"
#include "MAV2DIcon.h"
#include <QDebug>
......@@ -229,11 +230,21 @@ void HSIDisplay::renderOverlay()
painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
// Draw center indicator
QPolygonF p(3);
p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
drawPolygon(p, &painter);
// QPolygonF p(3);
// p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
// p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
// p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
// drawPolygon(p, &painter);
if (uas)
{
// Translate to center
painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
QColor uasColor = uas->getColor();
MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
// Translate back
painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
}
// ----------------------
......
......@@ -450,6 +450,18 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
qDebug() << "UPDATING WP" << wp->getId() << wp << __FILE__ << __LINE__;
if (uas == this->mav->getUASID())
{
// TODO The map widget is not yet aware of non-global, non-navigation WPs
if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->getAction() == MAV_ACTION_NAVIGATE)
{
// We're good, this is a global waypoint
// This is where the code below should be executed in
}
else
{
// We're screwed, this is not a global waypoint
qDebug() << "WARNING: The map widget is not prepared yet for non-navigation WPs" << __FILE__ << __LINE__;
}
int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
if (wpindex == -1) return;
// Create waypoint name
......@@ -467,8 +479,6 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
// Waypoint exists, update it
if(!waypointIsDrag)
{
qDebug() <<"indice WP= "<< wpindex <<"\n";
QPointF coordinate;
coordinate.setX(wp->getX());
coordinate.setY(wp->getY());
......@@ -482,6 +492,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
// Now update icon position
//mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
wpIcons.at(wpindex)->setCoordinate(coordinate);
wpIcons.at(wpindex)->setPen(mavPens.value(uas));
//mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
// Then waypoint line coordinate
Point* linesegment = NULL;
......@@ -505,12 +516,29 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
}
}
}
// Set unvisible
if (wp->getFrame() != MAV_FRAME_GLOBAL || !(wp->getAction() == MAV_ACTION_NAVIGATE
|| wp->getAction() == MAV_ACTION_LOITER
|| wp->getAction() == MAV_ACTION_LOITER_MAX_TIME
|| wp->getAction() == MAV_ACTION_LOITER_MAX_TURNS
|| wp->getAction() == MAV_ACTION_TAKEOFF
|| wp->getAction() == MAV_ACTION_LAND
|| wp->getAction() == MAV_ACTION_RETURN))
{
wpIcons.at(wpindex)->setVisible(false);
waypointPath->points().at(wpindex)->setVisible(false);
}
else
{
wpIcons.at(wpindex)->setVisible(true);
waypointPath->points().at(wpindex)->setVisible(true);
}
}
}
void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
{
if (!wpExists(coordinate))
//if (!wpExists(coordinate))
{
// Create waypoint name
QString str;
......@@ -820,7 +848,7 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
// Icon
//QPen* pointpen = new QPen(uasColor);
qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
p = new MAV2DIcon(uas, 68, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
uasIcons.insert(uas->getUASID(), p);
mc->layer("Waypoints")->addGeometry(p);
......
......@@ -203,7 +203,7 @@ void WaypointList::add()
{
// Create waypoint with last frame
Waypoint *last = waypoints.at(waypoints.size()-1);
wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(),
wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getAcceptanceRadius(),
last->getHoldTime(), last->getFrame(), last->getAction());
uas->getWaypointManager()->addWaypoint(wp);
}
......
This diff is collapsed.
......@@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project
namespace Ui {
class WaypointView;
}
class Ui_QGCCustomWaypointAction;
class WaypointView : public QWidget {
Q_OBJECT
......@@ -57,7 +58,9 @@ public slots:
void moveDown();
void remove();
void changedAutoContinue(int);
void updateFrameView(int frame);
void changedFrame(int state);
void updateActionView(int action);
void changedAction(int state);
void changedCurrent(int);
void updateValues(void);
......@@ -67,6 +70,9 @@ public slots:
protected:
virtual void changeEvent(QEvent *e);
Waypoint* wp;
// Special widgets extendending the
// waypoint view to mission capabilities
Ui_QGCCustomWaypointAction* customCommand;
private:
Ui::WaypointView *m_ui;
......
......@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1389</width>
<width>2208</width>
<height>39</height>
</rect>
</property>
......@@ -161,7 +161,7 @@ QProgressBar::chunk#thrustBar {
<property name="title">
<string/>
</property>
<layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0">
<layout class="QHBoxLayout" name="horizontalLayout" stretch="1,1,2,2,10,10,10,10,10,10,10,10,10,10,10,10,10,1,1,1,1">
<property name="spacing">
<number>2</number>
</property>
......@@ -202,7 +202,7 @@ QProgressBar::chunk#thrustBar {
<string>Waypoint Sequence Number</string>
</property>
<property name="text">
<string>TextLabel</string>
<string>ID</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
......@@ -223,6 +223,9 @@ QProgressBar::chunk#thrustBar {
<property name="statusTip">
<string>Action at Waypoint</string>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property>
</widget>
</item>
<item>
......@@ -239,6 +242,9 @@ QProgressBar::chunk#thrustBar {
<property name="statusTip">
<string>Coordinate frame</string>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property>
</widget>
</item>
<item>
......@@ -528,6 +534,16 @@ QProgressBar::chunk#thrustBar {
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="acceptanceSpinBox">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>0.200000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="holdTimeSpinBox">
<property name="sizePolicy">
......@@ -559,6 +575,19 @@ QProgressBar::chunk#thrustBar {
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="turnsSpinBox">
<property name="toolTip">
<string>Number of turns to loiter</string>
</property>
<property name="statusTip">
<string>Number of turns to loiter</string>
</property>
<property name="suffix">
<string> turns</string>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="takeOffAngleSpinBox">
<property name="sizePolicy">
......@@ -584,6 +613,16 @@ QProgressBar::chunk#thrustBar {
</property>
</widget>
</item>
<item>
<widget class="QWidget" name="customActionWidget" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="autoContinue">
<property name="toolTip">
......
......@@ -99,7 +99,11 @@ void MAV2DIcon::drawIcon(QPen* pen)
//qDebug() << "Painting ellipse" << radius/2-width << width;
//selPen->deleteLater();
}
drawAirframePolygon(airframe, painter, radius, iconColor, yaw);
}
void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw)
{
switch (airframe)
{
case UASInterface::QGC_AIRFRAME_PREDATOR:
......@@ -147,6 +151,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
painter.setPen(iconPen);
painter.drawPolygon(poly);
painter.rotate(-yawRotate);
}
break;
case UASInterface::QGC_AIRFRAME_MIKROKOPTER:
......@@ -186,6 +191,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
painter.setBrush(Qt::red);
painter.drawEllipse(front, radius/4/2, radius/4/2);
painter.rotate(-yawRotate);
}
break;
case UASInterface::QGC_AIRFRAME_EASYSTAR:
......@@ -243,6 +249,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
painter.setPen(iconPen);
painter.drawPolygon(poly);
painter.rotate(-yawRotate);
}
break;
case UASInterface::QGC_AIRFRAME_GENERIC:
......@@ -268,6 +275,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
painter.setPen(iconPen);
painter.drawPolygon(poly);
painter.rotate(-yawRotate);
}
break;
}
......
......@@ -61,6 +61,7 @@ public:
void drawIcon(QPen* pen);
void drawIcon() { drawIcon(mypen); }
static void drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw);
protected:
float yaw; ///< Yaw angle of the MAV
......
......@@ -90,7 +90,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getOrbit(),
wp->getAcceptanceRadius(),
fabs(wpZ));
sd->setShape(cylinder);
......
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>QGCCustomWaypointAction</class>
<widget class="QWidget" name="QGCCustomWaypointAction">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>791</width>
<height>25</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="spacing">
<number>5</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QSpinBox" name="commandSpinBox">
<property name="prefix">
<string>CMD </string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>255</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="param1SpinBox">
<property name="prefix">
<string>P1 </string>
</property>
<property name="minimum">
<double>-2147483647.000000000000000</double>
</property>
<property name="maximum">
<double>2147483647.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="param2SpinBox">
<property name="prefix">
<string>P2 </string>
</property>
<property name="minimum">
<double>-2147483647.000000000000000</double>
</property>
<property name="maximum">
<double>2147483647.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="param3SpinBox">
<property name="prefix">
<string>P3 </string>
</property>
<property name="minimum">
<double>-2147483647.000000000000000</double>
</property>
<property name="maximum">
<double>2147483647.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="param4SpinBox">
<property name="prefix">
<string>P4 </string>
</property>
<property name="minimum">
<double>-2147483647.000000000000000</double>
</property>
<property name="maximum">
<double>2147483647.000000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
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