Commit 676e0d02 authored by pixhawk's avatar pixhawk

Mission plan: support for pixhawk MAV_CMD_NAV_SWEEP

parent 2a1653ae
......@@ -246,7 +246,8 @@ FORMS += src/ui/MainWindow.ui \
src/ui/mission/QGCMissionNavLoiterTime.ui \
src/ui/mission/QGCMissionNavReturnToLaunch.ui \
src/ui/mission/QGCMissionNavLand.ui \
src/ui/mission/QGCMissionNavTakeoff.ui
src/ui/mission/QGCMissionNavTakeoff.ui \
src/ui/mission/QGCMissionNavSweep.ui
INCLUDEPATH += src \
src/ui \
src/ui/linechart \
......@@ -377,7 +378,8 @@ HEADERS += src/MG.h \
src/ui/mission/QGCMissionNavLoiterTime.h \
src/ui/mission/QGCMissionNavReturnToLaunch.h \
src/ui/mission/QGCMissionNavLand.h \
src/ui/mission/QGCMissionNavTakeoff.h
src/ui/mission/QGCMissionNavTakeoff.h \
src/ui/mission/QGCMissionNavSweep.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
......@@ -517,7 +519,8 @@ SOURCES += src/main.cc \
src/ui/mission/QGCMissionNavLoiterTime.cc \
src/ui/mission/QGCMissionNavReturnToLaunch.cc \
src/ui/mission/QGCMissionNavLand.cc \
src/ui/mission/QGCMissionNavTakeoff.cc
src/ui/mission/QGCMissionNavTakeoff.cc \
src/ui/mission/QGCMissionNavSweep.cc
# Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
......@@ -28,6 +28,7 @@
#include "QGCMissionNavReturnToLaunch.h"
#include "QGCMissionNavLand.h"
#include "QGCMissionNavTakeoff.h"
#include "QGCMissionNavSweep.h"
#include "QGCMissionConditionDelay.h"
#include "QGCMissionDoJump.h"
#include "QGCMissionOther.h"
......@@ -56,6 +57,7 @@ WaypointEditableView::WaypointEditableView(Waypoint* wp, QWidget* parent) :
MissionNavReturnToLaunchWidget = NULL;
MissionNavLandWidget = NULL;
MissionNavTakeoffWidget = NULL;
MissionNavSweepWidget = NULL;
MissionDoJumpWidget = NULL;
MissionConditionDelayWidget = NULL;
MissionOtherWidget = NULL;
......@@ -72,7 +74,10 @@ WaypointEditableView::WaypointEditableView(Waypoint* wp, QWidget* parent) :
//m_ui->comboBox_action->addItem(tr("NAV: Target"),MAV_CMD_NAV_TARGET);
m_ui->comboBox_action->addItem(tr("IF: Delay over"),MAV_CMD_CONDITION_DELAY);
//m_ui->comboBox_action->addItem(tr("IF: Yaw angle is"),MAV_CMD_CONDITION_YAW);
m_ui->comboBox_action->addItem(tr("DO: Jump to Index"),MAV_CMD_DO_JUMP);
m_ui->comboBox_action->addItem(tr("DO: Jump to Index"),MAV_CMD_DO_JUMP);
#ifdef MAVLINK_ENABLED_PIXHAWK
m_ui->comboBox_action->addItem(tr("NAV: Sweep"),MAV_CMD_NAV_SWEEP);
#endif
m_ui->comboBox_action->addItem(tr("Other"), MAV_CMD_ENUM_END);
// add frames
......@@ -174,6 +179,12 @@ void WaypointEditableView::updateActionView(int action)
case MAV_CMD_DO_JUMP:
if(MissionDoJumpWidget) MissionDoJumpWidget->show();
break;
#ifdef MAVLINK_ENABLED_PIXHAWK
case MAV_CMD_NAV_SWEEP:
if(MissionNavSweepWidget) MissionNavSweepWidget->show();
break;
#endif
default:
if(MissionOtherWidget) MissionOtherWidget->show();
viewMode = QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING;
......@@ -276,7 +287,15 @@ void WaypointEditableView::initializeActionView(int actionID)
m_ui->customActionWidget->layout()->addWidget(MissionDoJumpWidget);
}
break;
#ifdef MAVLINK_ENABLED_PIXHAWK
case MAV_CMD_NAV_SWEEP:
if (!MissionNavSweepWidget)
{
MissionNavSweepWidget = new QGCMissionNavSweep(this);
m_ui->customActionWidget->layout()->addWidget(MissionNavSweepWidget);
}
break;
#endif
case MAV_CMD_ENUM_END:
default:
if (!MissionOtherWidget)
......
......@@ -53,6 +53,7 @@ class QGCMissionNavLoiterTime;
class QGCMissionNavReturnToLaunch;
class QGCMissionNavLand;
class QGCMissionNavTakeoff;
class QGCMissionNavSweep;
class QGCMissionDoJump;
class QGCMissionConditionDelay;
class QGCMissionOther;
......@@ -105,6 +106,7 @@ protected:
QGCMissionNavReturnToLaunch* MissionNavReturnToLaunchWidget;
QGCMissionNavLand* MissionNavLandWidget;
QGCMissionNavTakeoff* MissionNavTakeoffWidget;
QGCMissionNavSweep* MissionNavSweepWidget;
QGCMissionDoJump* MissionDoJumpWidget;
QGCMissionConditionDelay* MissionConditionDelayWidget;
QGCMissionOther* MissionOtherWidget;
......
#include "QGCMissionNavSweep.h"
#include "ui_QGCMissionNavSweep.h"
#include "WaypointEditableView.h"
QGCMissionNavSweep::QGCMissionNavSweep(WaypointEditableView* WEV) :
QWidget(WEV),
ui(new Ui::QGCMissionNavSweep)
{
ui->setupUi(this);
this->WEV = WEV;
//Using UI to change WP:
connect(this->ui->radSpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam1(double)));
//connect(this->ui->acceptanceSpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam2(double)));
connect(this->ui->posN1SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam3(double)));//NED
connect(this->ui->posE1SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam4(double)));
connect(this->ui->posN2SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam5(double)));
connect(this->ui->posE2SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam6(double)));
connect(this->ui->posDSpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam7(double)));
connect(this->ui->lat1SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam3(double)));//Global
connect(this->ui->lon1SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam4(double)));
connect(this->ui->lat2SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam5(double)));
connect(this->ui->lon2SpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam6(double)));
connect(this->ui->altSpinBox, SIGNAL(valueChanged(double)),WEV,SLOT(changedParam7(double)));
//Reading WP to update UI:
connect(WEV,SIGNAL(frameBroadcast(MAV_FRAME)),this,SLOT(updateFrame(MAV_FRAME)));
connect(WEV,SIGNAL(param1Broadcast(double)),this->ui->radSpinBox,SLOT(setValue(double)));
//connect(WEV,SIGNAL(param2Broadcast(double)),this->ui->acceptanceSpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param3Broadcast(double)),this->ui->posN1SpinBox,SLOT(setValue(double)));//NED
connect(WEV,SIGNAL(param4Broadcast(double)),this->ui->posE1SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param5Broadcast(double)),this->ui->posN2SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param6Broadcast(double)),this->ui->posE2SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param7Broadcast(double)),this->ui->posDSpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param3Broadcast(double)),this->ui->lat1SpinBox,SLOT(setValue(double)));//Global
connect(WEV,SIGNAL(param4Broadcast(double)),this->ui->lon1SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param5Broadcast(double)),this->ui->lat2SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param6Broadcast(double)),this->ui->lon2SpinBox,SLOT(setValue(double)));
connect(WEV,SIGNAL(param7Broadcast(double)),this->ui->altSpinBox,SLOT(setValue(double)));
}
void QGCMissionNavSweep::updateFrame(MAV_FRAME frame)
{
switch(frame)
{
case MAV_FRAME_LOCAL_ENU:
case MAV_FRAME_LOCAL_NED:
this->ui->posN1SpinBox->show();
this->ui->posE1SpinBox->show();
this->ui->posN2SpinBox->show();
this->ui->posE2SpinBox->show();
this->ui->posDSpinBox->show();
this->ui->lat1SpinBox->hide();
this->ui->lon1SpinBox->hide();
this->ui->lat2SpinBox->hide();
this->ui->lon2SpinBox->hide();
this->ui->altSpinBox->hide();
break;
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
this->ui->posN1SpinBox->hide();
this->ui->posE1SpinBox->hide();
this->ui->posN2SpinBox->hide();
this->ui->posE2SpinBox->hide();
this->ui->posDSpinBox->hide();
this->ui->lat1SpinBox->show();
this->ui->lon1SpinBox->show();
this->ui->lat2SpinBox->show();
this->ui->lon2SpinBox->show();
this->ui->altSpinBox->show();
break;
default:
break;
}
}
QGCMissionNavSweep::~QGCMissionNavSweep()
{
delete ui;
}
#ifndef QGCMISSIONNAVSWEEP_H
#define QGCMISSIONNAVSWEEP_H
#include <QWidget>
#include "WaypointEditableView.h"
namespace Ui {
class QGCMissionNavSweep;
}
class QGCMissionNavSweep : public QWidget
{
Q_OBJECT
public:
explicit QGCMissionNavSweep(WaypointEditableView* WEV);
~QGCMissionNavSweep();
public slots:
void updateFrame(MAV_FRAME);
protected:
WaypointEditableView* WEV;
private:
Ui::QGCMissionNavSweep *ui;
};
#endif // QGCMISSIONNAVSWEEP_H
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>QGCMissionNavSweep</class>
<widget class="QWidget" name="QGCMissionNavSweep">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>2208</width>
<height>37</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="spacing">
<number>5</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QDoubleSpinBox" name="posN1SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>X coordinate of the first sweep area corner</string>
</property>
<property name="statusTip">
<string>X coordinate of the first sweep area corner</string>
</property>
<property name="wrapping">
<bool>false</bool>
</property>
<property name="frame">
<bool>true</bool>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(1)N </string>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>-10000.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="posE1SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Y coordinate of the first sweep area corner</string>
</property>
<property name="statusTip">
<string>Y coordinate of the first sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(1)E </string>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>-10000.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="posN2SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>X coordinate of the second sweep area corner</string>
</property>
<property name="statusTip">
<string>X coordinate of the second sweep area corner</string>
</property>
<property name="wrapping">
<bool>false</bool>
</property>
<property name="frame">
<bool>true</bool>
</property>
<property name="accelerated">
<bool>true</bool>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(2)N </string>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>-10000.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="posE2SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Y coordinate of the second sweep area corner</string>
</property>
<property name="statusTip">
<string>Y coordinate of the second sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(2)E </string>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>-10000.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="posDSpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Z coordinate (local frame, negative)</string>
</property>
<property name="statusTip">
<string/>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>D </string>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>-10000.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="lat1SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Latitude of the first sweep area corner</string>
</property>
<property name="statusTip">
<string>Latitude of the first sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(1)lat </string>
</property>
<property name="suffix">
<string>°</string>
</property>
<property name="decimals">
<number>7</number>
</property>
<property name="minimum">
<double>-90.000000000000000</double>
</property>
<property name="maximum">
<double>90.000000000000000</double>
</property>
<property name="singleStep">
<double>0.000010000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="lon1SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Longitude of the first sweep area corner</string>
</property>
<property name="statusTip">
<string>Longitude of the first sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(1)lon </string>
</property>
<property name="suffix">
<string>°</string>
</property>
<property name="decimals">
<number>7</number>
</property>
<property name="minimum">
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>0.000010000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="lat2SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Latitude of the second sweep area corner</string>
</property>
<property name="statusTip">
<string>Latitude of the second sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(2)lat </string>
</property>
<property name="suffix">
<string>°</string>
</property>
<property name="decimals">
<number>7</number>
</property>
<property name="minimum">
<double>-90.000000000000000</double>
</property>
<property name="maximum">
<double>90.000000000000000</double>
</property>
<property name="singleStep">
<double>0.000010000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="lon2SpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Longitude of the second sweep area corner</string>
</property>
<property name="statusTip">
<string>Longitude of the second sweep area corner</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>(2)lon </string>
</property>
<property name="suffix">
<string>°</string>
</property>
<property name="decimals">
<number>7</number>
</property>
<property name="minimum">
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>0.000010000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="altSpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Altitude</string>
</property>
<property name="statusTip">
<string>Altitude</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="prefix">
<string>alt </string>
</property>
<property name="suffix">
<string>m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-100000.000000000000000</double>
</property>
<property name="maximum">
<double>100000.000000000000000</double>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="radSpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Radius of the circle under the MAV covered by the camera</string>
</property>
<property name="statusTip">
<string>Radius of the circle under the MAV covered by the camera</string>
</property>
<property name="keyboardTracking">
<bool>false</bool>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>0.200000000000000</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