Skip to content
GitLab
Commit c0b8cd99 authored by lm's avatar lm
Browse files

Merge branch 'experimental' of pixhawk.ethz.ch:qgroundcontrol into experimental

parents 18128c94 2f721788
Show whitespace changes
Inline Side-by-side
src/uas/UAS.cc
View file @ c0b8cd99
......@@ -1290,6 +1290,19 @@ void UAS::setParameter(int component, QString id, float value)
sendMessage(msg);
}
/**
* Sets an action
*
**/
void UAS::setAction(MAV_ACTION action)
{
mavlink_message_t msg;
mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action);
// Send message twice to increase chance that it reaches its goal
sendMessage(msg);
sendMessage(msg);
}
/**
* Launches the system
*
......
src/uas/UAS.h
View file @ c0b8cd99
......@@ -174,6 +174,9 @@ public:
void setAutopilotType(int apType) { autopilot = apType;}
public slots:
/** @brief Sets an action **/
void setAction(MAV_ACTION action);
/** @brief Launches the system **/
void launch();
/** @brief Write this waypoint to the list of waypoints */
......
src/uas/UASInterface.h
View file @ c0b8cd99
......@@ -162,6 +162,9 @@ public:
public slots:
/** @brief Sets an action **/
virtual void setAction(MAV_ACTION action) = 0;
/** @brief Launches the system/Liftof **/
virtual void launch() = 0;
/** @brief Set a new waypoint **/
......
src/ui/designer/QGCActionButton.cc
View file @ c0b8cd99
#include "QGCActionButton.h"
#include "ui_QGCActionButton.h"
#include "MAVLinkProtocol.h"
#include "UASManager.h"
const char* kActionLabels[MAV_ACTION_NB] =
{"HOLD",
"START MOTORS",
"LAUNCH",
"RETURN",
"EMERGENCY LAND",
"EMERGENCY KILL",
"CONFIRM KILL",
"CONTINUE",
"STOP MOTORS",
"HALT",
"SHUTDOWN",
"REBOOT",
"SET MANUAL",
"SET AUTO",
"READ STORAGE",
"WRITE STORAGE",
"CALIBRATE RC",
"CALIBRATE GYRO",
"CALIBRATE MAG",
"CALIBRATE ACC",
"CALIBRATE PRESSURE",
"START REC",
"PAUSE REC",
"STOP REC",
"TAKEOFF",
"NAVIGATE",
"LAND",
"LOITER",
"SET ORIGIN",
"RELAY ON",
"RELAY OFF",
"GET IMAGE",
"START VIDEO",
"STOP VIDEO",
"RESET MAP"};
QGCActionButton::QGCActionButton(QWidget *parent) :
QGCToolWidgetItem(parent),
ui(new Ui::QGCActionButton)
ui(new Ui::QGCActionButton),
uas(NULL)
{
ui->setupUi(this);
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
this, SLOT(setActiveUAS(UASInterface*)));
connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction()));
connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode()));
connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString)));
connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString)));
endEditMode();
// add action labels to combobox
for (int i = 0; i < MAV_ACTION_NB; i++)
{
ui->editActionComboBox->addItem(kActionLabels[i]);
}
}
QGCActionButton::~QGCActionButton()
......@@ -15,6 +69,22 @@ QGCActionButton::~QGCActionButton()
delete ui;
}
void QGCActionButton::sendAction()
{
if (uas)
{
MAV_ACTION action = static_cast<MAV_ACTION>(
ui->editActionComboBox->currentIndex());
uas->setAction(action);
}
}
void QGCActionButton::setActionButtonName(QString text)
{
ui->actionButton->setText(text);
}
void QGCActionButton::startEditMode()
{
ui->editActionComboBox->show();
......@@ -30,3 +100,8 @@ void QGCActionButton::endEditMode()
ui->editFinishButton->hide();
isInEditMode = false;
}
void QGCActionButton::setActiveUAS(UASInterface *uas)
{
this->uas = uas;
}
src/ui/designer/QGCActionButton.h
View file @ c0b8cd99
......@@ -7,6 +7,8 @@ namespace Ui {
class QGCActionButton;
}
class UASInterface;
class QGCActionButton : public QGCToolWidgetItem
{
Q_OBJECT
......@@ -16,11 +18,17 @@ public:
~QGCActionButton();
public slots:
void sendAction();
void setActionButtonName(QString text);
void startEditMode();
void endEditMode();
private slots:
void setActiveUAS(UASInterface* uas);
private:
Ui::QGCActionButton *ui;
UASInterface* uas;
};
#endif // QGCACTIONBUTTON_H
src/ui/designer/QGCActionButton.ui
View file @ c0b8cd99
......@@ -21,13 +21,6 @@
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="actionButton">
<property name="text">
<string>Button name</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QComboBox" name="editActionComboBox"/>
</item>
......@@ -59,26 +52,17 @@
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="actionButton">
<property name="text">
<string>Button name</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections>
<connection>
<sender>editButtonName</sender>
<signal>textChanged(QString)</signal>
<receiver>actionButton</receiver>
<slot>setWindowTitle(QString)</slot>
<hints>
<hint type="sourcelabel">
<x>310</x>
<y>22</y>
</hint>
<hint type="destinationlabel">
<x>310</x>
<y>55</y>
</hint>
</hints>
</connection>
<connection>
<sender>editNameLabel</sender>
<signal>textChanged(QString)</signal>
......@@ -86,11 +70,11 @@
<slot>setText(QString)</slot>
<hints>
<hint type="sourcelabel">
<x>116</x>
<x>114</x>
<y>22</y>
</hint>
<hint type="destinationlabel">
<x>116</x>
<x>114</x>
<y>55</y>
</hint>
</hints>
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ c0b8cd99
......@@ -56,6 +56,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, displayRGBD2D(false)
, displayRGBD3D(false)
, enableRGBDColor(true)
, enableTarget(false)
, followCamera(true)
, enableFreenect(false)
, frame(MAV_FRAME_GLOBAL)
......@@ -87,6 +88,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
waypointGroupNode->init();
rollingMap->addChild(waypointGroupNode);
// generate target model
targetNode = createTarget();
rollingMap->addChild(targetNode);
#ifdef QGC_LIBFREENECT_ENABLED
freenect.reset(new Freenect());
enableFreenect = freenect->init();
......@@ -236,6 +241,36 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
}
}
void
Pixhawk3DWidget::selectTarget(void)
{
if (uas)
{
if (frame == MAV_FRAME_GLOBAL)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
target.set(cursorWorldCoords.first, cursorWorldCoords.second);
}
else if (frame == MAV_FRAME_LOCAL)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
target.set(cursorWorldCoords.first, cursorWorldCoords.second);
}
uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
enableTarget = true;
}
}
void
Pixhawk3DWidget::insertWaypoint(void)
{
......@@ -539,8 +574,6 @@ Pixhawk3DWidget::display(void)
lastRobotZ = robotZ;
recenterCamera(robotY, robotX, -robotZ);
return;
}
if (followCamera)
......@@ -572,6 +605,11 @@ Pixhawk3DWidget::display(void)
updateWaypoints();
}
if (enableTarget)
{
updateTarget(robotX, robotY);
}
#ifdef QGC_LIBFREENECT_ENABLED
if (enableFreenect && (displayRGBD2D || displayRGBD3D))
{
......@@ -586,6 +624,7 @@ Pixhawk3DWidget::display(void)
rollingMap->setChildValue(trailNode, displayTrail);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
rollingMap->setChildValue(targetNode, enableTarget);
if (enableFreenect)
{
egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
......@@ -843,6 +882,26 @@ Pixhawk3DWidget::createRGBD3D(void)
return geode;
}
osg::ref_ptr<osg::Node>
Pixhawk3DWidget::createTarget(void)
{
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
sphereGeode->addDrawable(sphereDrawable);
sphereGeode->setName("Target");
pat->addChild(sphereGeode);
return pat;
}
void
Pixhawk3DWidget::setupHUD(void)
{
......@@ -1118,6 +1177,14 @@ Pixhawk3DWidget::updateWaypoints(void)
waypointGroupNode->update(frame, uas);
}
void
Pixhawk3DWidget::updateTarget(double robotX, double robotY)
{
osg::PositionAttitudeTransform* pat =
static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
}
float colormap_jet[128][3] =
{
{0.0f,0.0f,0.53125f},
......@@ -1336,7 +1403,6 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
std::string nodeName = it->nodePath[i]->getName();
if (nodeName.substr(0, 2).compare("wp") == 0)
{
qDebug() << nodeName.c_str() << "Got!!";
return atoi(nodeName.substr(2).c_str());
}
}
......@@ -1347,12 +1413,40 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
return -1;
}
bool
Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
{
if (getSceneData() != NULL)
{
osgUtil::LineSegmentIntersector::Intersections intersections;
if (computeIntersections(mouseX, height() - mouseY, intersections))
{
for (osgUtil::LineSegmentIntersector::Intersections::iterator
it = intersections.begin(); it != intersections.end(); it++)
{
for (uint i = 0 ; i < it->nodePath.size(); ++i)
{
std::string nodeName = it->nodePath[i]->getName();
if (nodeName.compare("Target") == 0)
{
return true;
}
}
}
}
}
return false;
}
void
Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
{
QMenu menu;
menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint()));
menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
menu.addAction("Select target", this, SLOT(selectTarget()));
menu.exec(cursorPos);
}
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ c0b8cd99
......@@ -71,6 +71,7 @@ private slots:
void recenter(void);
void toggleFollowCamera(int state);
void selectTarget(void);
void insertWaypoint(void);
void moveWaypoint(void);
void setWaypoint(void);
......@@ -101,6 +102,7 @@ private:
osg::ref_ptr<osg::Geode> createTrail(void);
osg::ref_ptr<Imagery> createMap(void);
osg::ref_ptr<osg::Geode> createRGBD3D(void);
osg::ref_ptr<osg::Node> createTarget(void);
void setupHUD(void);
void resizeHUD(void);
......@@ -112,11 +114,13 @@ private:
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateWaypoints(void);
void updateTarget(double robotX, double robotY);
#ifdef QGC_LIBFREENECT_ENABLED
void updateRGBD(void);
#endif
int findWaypoint(int mouseX, int mouseY);
bool findTarget(int mouseX, int mouseY);
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
......@@ -134,6 +138,7 @@ private:
bool displayRGBD2D;
bool displayRGBD3D;
bool enableRGBDColor;
bool enableTarget;
bool followCamera;
......@@ -154,6 +159,7 @@ private:
osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
osg::ref_ptr<Imagery> mapNode;
osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
osg::ref_ptr<osg::Node> targetNode;
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_LIBFREENECT_ENABLED
QScopedPointer<Freenect> freenect;
......@@ -163,6 +169,7 @@ private:
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
MAV_FRAME frame;
osg::Vec2d target;
double lastRobotX, lastRobotY, lastRobotZ;
};
......
Supports Markdown
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