Commit 017b5ae1 authored by pixhawk's avatar pixhawk

Consolidated merged state from lm, Lionel, malife and godbolt. Tested and supposedly stable.

parents a227e116 a84920af
......@@ -97,6 +97,36 @@ namespace qmapcontrol
geometries.clear();
}
Geometry* Layer::get_Geometry(int index)
{
if(geometrySelected)
{
return geometrySelected;
}
else
{
for(int i = 0; i <= geometries.size(); i++)
{
Geometry *geometry = geometries[i];
if(geometry->name() == QString::number(index))
{
return geometry;
}
}
// foreach(Geometry *geometry, geometries)
// {
// if(geometry->name() == QString::number(index))
// {
// return geometry;
// }
// }
}
}
bool Layer::isVisible() const
{
return visible;
......@@ -323,3 +353,5 @@ namespace qmapcontrol
mapAdapter = mapadapter;
}
}
......@@ -200,6 +200,12 @@ namespace qmapcontrol
*/
void setVisible(bool visible);
//! get geometry selected by index
/*!
* @param index of geometry selected
*/
Geometry* get_Geometry(int index);
};
}
#endif
......@@ -309,6 +309,7 @@ namespace qmapcontrol
click.y()-screen_middle.y()+layermanager->getMapmiddle_px().y());
// image coordinate to world coordinate
return layermanager->layer()->mapadapter()->displayToCoordinate(displayToImage);
}
void MapControl::updateRequest(QRect rect)
......
......@@ -159,7 +159,6 @@ HEADERS += src/MG.h \
src/ui/map/Waypoint2DIcon.h \
src/ui/map/MAV2DIcon.h \
src/ui/QGCRemoteControlView.h \
src/WaypointGlobal.h \
src/ui/WaypointGlobalView.h \
src/ui/map3D/Q3DWidget.h \
src/ui/map3D/CheetahModel.h \
......@@ -233,8 +232,7 @@ SOURCES += src/main.cc \
src/ui/map/Waypoint2DIcon.cc \
src/ui/map/MAV2DIcon.cc \
src/ui/QGCRemoteControlView.cc \
src/WaypointGlobal.cpp \
src/ui/WaypointGlobalView.cpp \
src/ui/WaypointGlobalView.cc \
src/ui/map3D/Q3DWidget.cc \
src/ui/map3D/CheetahModel.cc \
src/ui/map3D/CheetahGL.cc \
......
......@@ -81,6 +81,7 @@ public slots:
void setOrbit(float orbit);
void setHoldTime(int holdTime);
//for QDoubleSpin
void setX(double x);
void setY(double y);
......
#include "WaypointGlobal.h"
#include <QPointF>
WaypointGlobal::WaypointGlobal(const QPointF coordinate):
Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime)
{
coordinateWP = coordinate;
}
#ifndef WAYPOINTGLOBAL_H
#define WAYPOINTGLOBAL_H
#include "Waypoint.h"
#include <QPointF>
class WaypointGlobal: public Waypoint {
Q_OBJECT
public:
WaypointGlobal(const QPointF coordinate);
public slots:
// void set_latitud(double latitud);
// void set_longitud(double longitud);
// double get_latitud();
// double get_longitud();
private:
QPointF coordinateWP;
};
#endif // WAYPOINTGLOBAL_H
......@@ -53,7 +53,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
}
#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES
#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES_QGC
case MAVLINK_MSG_ID_CPU_LOAD:
{
......
......@@ -171,6 +171,9 @@ void MainWindow::buildWidgets()
rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
headUpDockWidget = new QDockWidget(tr("Control Indicator"), this);
headUpDockWidget->setWidget( new HUD(320, 240, this));
// Dialogue widgets
//FIXME: free memory in destructor
joystick = new JoystickInput();
......@@ -203,8 +206,13 @@ void MainWindow::connectWidgets()
// add Waypoint widget in the WaypointList widget when mouse clicked
connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF)));
// it notifies that a waypoint global goes to do create
connect(mapWidget, SIGNAL(createGlobalWP(bool)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool)));
connect(mapWidget, SIGNAL(createGlobalWP(bool, QPointF)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool, QPointF)));
connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) );
// it notifies that a waypoint global goes to do create and a map graphic too
connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
// it notifies that a waypoint global change its position by spinBox on Widget WaypointView
connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float)));
}
}
......@@ -213,7 +221,6 @@ void MainWindow::arrangeCenterStack()
QStackedWidget *centerStack = new QStackedWidget(this);
if (!centerStack) return;
if (linechartWidget) centerStack->addWidget(linechartWidget);
if (protocolWidget) centerStack->addWidget(protocolWidget);
if (mapWidget) centerStack->addWidget(mapWidget);
......@@ -362,7 +369,7 @@ void MainWindow::connectActions()
connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView()));
connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView()));
connect(ui.actionStyleConfig, SIGNAL(triggered()), this, SLOT(reloadStylesheet()));
connect(ui.actionGlobalOperatorView, SIGNAL(triggered()), this, SLOT(loadGlobalOperatorView()));
connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp()));
connect(ui.actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits()));
connect(ui.actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap()));
......@@ -808,6 +815,7 @@ void MainWindow::loadOperatorView()
centerStack->setCurrentWidget(mapWidget);
}
}
// UAS CONTROL
if (controlDockWidget)
{
......@@ -865,6 +873,83 @@ void MainWindow::loadOperatorView()
this->show();
}
void MainWindow::loadGlobalOperatorView()
{
clearView();
// MAP
if (mapWidget)
{
QStackedWidget *centerStack = dynamic_cast<QStackedWidget*>(centralWidget());
if (centerStack)
{
centerStack->setCurrentWidget(mapWidget);
}
}
// UAS CONTROL
if (controlDockWidget)
{
addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget);
controlDockWidget->show();
}
// UAS LIST
if (listDockWidget)
{
addDockWidget(Qt::BottomDockWidgetArea, listDockWidget);
listDockWidget->show();
}
// UAS STATUS
if (infoDockWidget)
{
addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget);
infoDockWidget->show();
}
// WAYPOINT LIST
if (waypointsDockWidget)
{
addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget);
waypointsDockWidget->show();
}
// // HORIZONTAL SITUATION INDICATOR
// if (hsiDockWidget)
// {
// HSIDisplay* hsi = dynamic_cast<HSIDisplay*>( hsiDockWidget->widget() );
// if (hsi)
// {
// addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget);
// hsiDockWidget->show();
// hsi->start();
// }
// }
// PROCESS CONTROL
if (watchdogControlDockWidget)
{
addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget);
watchdogControlDockWidget->show();
}
// HEAD UP DISPLAY
if (headUpDockWidget)
{
addDockWidget(Qt::RightDockWidgetArea, headUpDockWidget);
// FIXME Replace with default ->show() call
HUD* hud = dynamic_cast<HUD*>(headUpDockWidget->widget());
if (hud)
{
headUpDockWidget->show();
hud->start();
}
}
}
void MainWindow::load3DView()
{
clearView();
......
......@@ -122,6 +122,8 @@ public slots:
void loadDataView();
/** @brief Load data view, allowing to plot flight data */
void loadDataView(QString fileName);
/** @brief Load view for global operator, allowing to flight on earth */
void loadGlobalOperatorView();
/** @brief Show the online help for users */
void showHelp();
......@@ -174,6 +176,7 @@ protected:
QPointer<QDockWidget> headDown1DockWidget;
QPointer<QDockWidget> headDown2DockWidget;
QPointer<QDockWidget> watchdogControlDockWidget;
QPointer<QDockWidget> headUpDockWidget;
QPointer<QDockWidget> hsiDockWidget;
QPointer<QDockWidget> rcViewDockWidget;
......
......@@ -76,6 +76,7 @@
<addaction name="actionPilotView"/>
<addaction name="actionOperatorView"/>
<addaction name="action3DView"/>
<addaction name="actionGlobalOperatorView"/>
<addaction name="separator"/>
<addaction name="actionShow_MAVLink_view"/>
<addaction name="actionShow_data_analysis_view"/>
......@@ -308,6 +309,15 @@
<string>Credits / Developers</string>
</property>
</action>
<action name="actionGlobalOperatorView">
<property name="icon">
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/applications-internet.svg</normaloff>:/images/categories/applications-internet.svg</iconset>
</property>
<property name="text">
<string>Show Global operator view</string>
</property>
</action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources>
......
......@@ -47,10 +47,13 @@ MapWidget::MapWidget(QWidget *parent) :
zoomLevel(0),
uasIcons(),
uasTrails(),
mav(NULL),
m_ui(new Ui::MapWidget)
{
m_ui->setupUi(this);
waypointIsDrag = false;
// Accept focus by clicking or keyboard
this->setFocusPolicy(Qt::StrongFocus);
......@@ -314,8 +317,9 @@ void MapWidget::createPathButtonClicked(bool checked)
this->setCursor(Qt::PointingHandCursor);
mc->setMouseMode(qmapcontrol::MapControl::None);
// emit signal start to create a Waypoint global
emit createGlobalWP(true);
emit createGlobalWP(true, mc->currentCoordinate());
// // Clear the previous WP track
// // TODO: Move this to an actual clear track button and add a warning dialog
......@@ -337,9 +341,11 @@ void MapWidget::createPathButtonClicked(bool checked)
}
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate){
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
{
if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()){
if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked())
{
// Create waypoint name
QString str;
......@@ -373,18 +379,45 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
}
}
void MapWidget::captureGeometryClick(Geometry* geom, QPoint point){
void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
{
// Create waypoint name
QString str;
str = QString("%1").arg(path->numberOfPoints());
// create the WP and set everything in the LineString to display the path
CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
mc->layer("Waypoints")->addGeometry(tempCirclePoint);
Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
wps.append(tempPoint);
path->addPoint(tempPoint);
wpIndex.insert(str,tempPoint);
// Refresh the screen
mc->updateRequestNew();
//// // emit signal mouse was clicked
// emit captureMapCoordinateClick(coordinate);
}
void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
{
Q_UNUSED(geom);
Q_UNUSED(point);
mc->setMouseMode(qmapcontrol::MapControl::None);
}
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate){
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
Q_UNUSED(coordinate);
waypointIsDrag = true;
// Refresh the screen
mc->updateRequestNew();
......@@ -412,6 +445,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate){
void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
{
waypointIsDrag = false;
mc->setMouseMode(qmapcontrol::MapControl::Panning);
......@@ -603,10 +637,35 @@ void MapWidget::clearPath()
wpIndex.clear();
mc->updateRequestNew();
// si el boton de crear wp globales esta activo desactivarlo llamando a su evento clicket
if(createPath->isChecked())
{
createPath->click();
}
}
void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon)
{
if(!waypointIsDrag)
{
qDebug() <<"indice WP= "<<index <<"\n";
QPointF coordinate;
coordinate.setX(lon);
coordinate.setY(lat);
Point* point2Find;
point2Find = wpIndex[QString::number(index)];
point2Find->setCoordinate(coordinate);
point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(index));
point2Find->setCoordinate(coordinate);
// Refresh the screen
mc->updateRequestNew();
}
}
......@@ -66,6 +66,7 @@ public slots:
//ROCA
void clearPath();
void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon);
protected:
void changeEvent(QEvent* e);
......@@ -106,6 +107,7 @@ protected:
protected slots:
void captureMapClick (const QMouseEvent* event, const QPointF coordinate);
void createWaypointGraphAtMap (const QPointF coordinate);
void createPathButtonClicked(bool checked);
void captureGeometryClick(Geometry*, QPoint);
void mapproviderSelected(QAction* action);
......@@ -114,10 +116,11 @@ protected:
signals:
//void movePoint(QPointF newCoord);
void captureMapCoordinateClick(const QPointF coordinate); //ROCA
void createGlobalWP(bool value);
void createGlobalWP(bool value, QPointF centerCoordinate);
void sendGeometryEndDrag(const QPointF coordinate, const int index);
......@@ -127,6 +130,7 @@ private:
QHash <QString, qmapcontrol::Point*> wpIndex;
qmapcontrol::LineString* path;
QPen* pointPen;
bool waypointIsDrag;
};
#endif // MAPWIDGET_H
......@@ -12,14 +12,30 @@ WaypointGlobalView::WaypointGlobalView(Waypoint* wp,QWidget *parent) :
ui->m_orbitalSpinBox->hide();
// Read values and set user interface
updateValues();
connect(ui->m_orbitalSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double)));
connect(ui->m_heigthSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double)));
connect(ui->m_orbitCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeOrbitalState(int)));
//for spinBox Latitude
connect(ui->m_latitudGrados_spinBox, SIGNAL(valueChanged(int)), this, SLOT(updateLatitudeWP(int)));
connect(ui->m_latitudMinutos_spinBox, SIGNAL(valueChanged(double)), this, SLOT(updateLatitudeMinuteWP(double)));
connect(ui->m_dirLatitudeN_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLatitudeWP()));
connect(ui->m_dirLatitudeS_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLatitudeWP()));
//for spinBox Longitude
connect(ui->m_longitudGrados_spinBox, SIGNAL(valueChanged(int)), this, SLOT(updateLongitudeWP(int)));
connect(ui->m_longitudMinutos_spinBox, SIGNAL(valueChanged(double)), this, SLOT(updateLongitudeMinuteWP(double)));
connect(ui->m_dirLongitudeW_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLongitudeWP()));
connect(ui->m_dirLongitudeE_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLongitudeWP()));
connect(ui->m_orbitCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeOrbitalState(int)));
// Read values and set user interface
updateValues();
// connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
......@@ -48,9 +64,46 @@ WaypointGlobalView::~WaypointGlobalView()
void WaypointGlobalView::updateValues()
{
ui->m_latitudtextEdit->setText(getLatitudString(wp->getY()));
ui->m_longitudtextEdit->setText(getLongitudString(wp->getX()));
ui->idWP_label->setText(QString("%1").arg(wp->getId()));\
int gradoLat, gradoLon;
float minLat, minLon;
QString dirLat, dirLon;
getLatitudeGradoMin(wp->getY(), &gradoLat, &minLat, &dirLat);
getLongitudGradoMin(wp->getX(), &gradoLon, &minLon, &dirLon);
//latitude on spinBox
ui->m_latitudGrados_spinBox->setValue(gradoLat);
ui->m_latitudMinutos_spinBox->setValue(minLat);
if(dirLat == "N")
{
ui->m_dirLatitudeN_radioButton->setChecked(true);
ui->m_dirLatitudeS_radioButton->setChecked(false);
}
else
{
ui->m_dirLatitudeS_radioButton->setChecked(true);
ui->m_dirLatitudeN_radioButton->setChecked(false);
}
//longitude on spinBox
ui->m_longitudGrados_spinBox->setValue(gradoLon);
ui->m_longitudMinutos_spinBox->setValue(minLon);
if(dirLon == "W")
{
ui->m_dirLongitudeW_radioButton->setChecked(true);
ui->m_dirLongitudeE_radioButton->setChecked(false);
}
else
{
ui->m_dirLongitudeE_radioButton->setChecked(true);
ui->m_dirLongitudeW_radioButton->setChecked(false);
}
ui->idWP_label->setText(QString("WP-%1").arg(wp->getId()));
}
......@@ -152,4 +205,258 @@ void WaypointGlobalView::changeOrbitalState(int state)
}
}
void WaypointGlobalView::getLatitudeGradoMin(float latitud, int *gradoLat, float *minLat, QString *dirLat)
{
float minutos = 0;
float grados = 0;
float entero = 0;
float dec = 0;
if (latitud<0){*dirLat="S"; latitud = latitud * -1;}
else {*dirLat="N";}
if(latitud< 90 || latitud > -90)
{
dec = latitud - (entero = ::floor(latitud));;
minutos = dec * 60;
grados = entero;
if(grados < 0) grados = grados * (-1);
if(minutos < 0) minutos = minutos * (-1);
*gradoLat = grados;
*minLat = minutos;
}
else
{
*gradoLat = -1;
*minLat = -1;
*dirLat="N/A";
}
}
void WaypointGlobalView::getLongitudGradoMin(float longitud, int *gradoLon, float *minLon, QString *dirLon)
{
float minutos = 0;
float grados = 0;
float entero = 0;
float dec = 0;
if (longitud<0){*dirLon="W"; longitud = longitud * -1;}
else {*dirLon="E";}
if(longitud<180 || longitud > -180)
{
dec = longitud - (entero = ::floor(longitud));;
minutos = dec * 60;
grados = entero;
if(grados < 0) grados = grados * (-1);
if(minutos < 0) minutos = minutos * (-1);
*gradoLon = grados;
*minLon = minutos;
}
else
{
*gradoLon = -1;
*minLon = -1;
*dirLon="N/A";
}
}
void WaypointGlobalView::updateCoordValues(float lat, float lon)
{
}
void WaypointGlobalView::updateLatitudeWP(int value)
{
Q_UNUSED(value);
int gradoLat;
float minLat;
float Latitud;
QString dirLat;
gradoLat = ui->m_latitudGrados_spinBox->value();
minLat = ui->m_latitudMinutos_spinBox->value();
if(ui->m_dirLatitudeN_radioButton->isChecked())
{
dirLat = "N";
}
else
{
dirLat = "S";
}
//dirLat = ui->m_dirLatitud_label->text();
Latitud = gradoLat + (minLat/60);
if(dirLat == "S"){Latitud = Latitud * -1;}
wp->setY(Latitud);
//emit signal waypoint position was changed
emit changePositionWP(wp);
}
void WaypointGlobalView::updateLatitudeMinuteWP(double value)
{
Q_UNUSED(value);
int gradoLat;
float minLat;
float Latitud;
QString dirLat;
gradoLat = ui->m_latitudGrados_spinBox->value();
minLat = ui->m_latitudMinutos_spinBox->value();
//dirLat = ui->m_dirLatitud_label->text();
if(ui->m_dirLatitudeN_radioButton->isChecked())
{
dirLat = "N";
}
else
{
dirLat = "S";
}
Latitud = gradoLat + (minLat/60);
if(dirLat == "S"){Latitud = Latitud * -1;}
wp->setY(Latitud);
//emit signal waypoint position was changed
emit changePositionWP(wp);
}
void WaypointGlobalView::updateLongitudeWP(int value)
{
Q_UNUSED(value);
int gradoLon;
float minLon;
float Longitud;
QString dirLon;
gradoLon = ui->m_longitudGrados_spinBox->value();
minLon = ui->m_longitudMinutos_spinBox->value();
// dirLon = ui->m_dirLongitud_label->text();
if(ui->m_dirLongitudeW_radioButton->isChecked())
{
dirLon = "W";
}
else
{
dirLon = "E";
}
Longitud = gradoLon + (minLon/60);
if(dirLon == "W"){Longitud = Longitud * -1;}
wp->setX(Longitud);
//emit signal waypoint position was changed
emit changePositionWP(wp);
}