diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 5d088cc7aa4074ea32f5cd6b2f0e15f907c5b0fe..98ea0f66033f14bbc2875e12558aa9401b0c3ac4 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -315,6 +315,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) currLossCounter = 0; currReceiveCounter = 0; emit receiveLossChanged(message.sysid, receiveLoss); + qDebug() << "LOSSCHANGED" << message.sysid<<" "<setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"); controlDockWidget->setWidget( new UASControlWidget(this) ); addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); - } - - if (!controlParameterWidget) - { - controlParameterWidget = new QDockWidget(tr("Control Parameters"), this); - controlParameterWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_PARAMETERWIDGET"); - controlParameterWidget->setWidget( new UASControlParameters(this) ); - addToToolsMenu (controlParameterWidget, tr("Control Parameters"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL_PARAM, Qt::LeftDockWidgetArea); - } + } if (!listDockWidget) { @@ -401,6 +393,14 @@ void MainWindow::buildCommonWidgets() dataplotWidget = new QGCDataPlot2D(this); addToCentralWidgetsMenu (dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); } + + if (!controlParameterWidget) + { + controlParameterWidget = new QDockWidget(tr("Control Parameters"), this); + controlParameterWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_PARAMETERWIDGET"); + controlParameterWidget->setWidget( new UASControlParameters(this) ); + addToToolsMenu (controlParameterWidget, tr("Control Parameters"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL_PARAM, Qt::LeftDockWidgetArea); + } } diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 543bc900982f69fa60f90e45013e07673a1cfcca..a498d1a33a5e444f3494d781f266ebbc04932292 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -93,6 +93,8 @@ void MapWidget::init() geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); mc->addLayer(geomLayer); + homePosition = new qmapcontrol::GeometryLayer("Station", mapadapter); + mc->addLayer(homePosition); // @@ -183,11 +185,17 @@ void MapWidget::init() goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to")); goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to")); + setHome = new QPushButton(QIcon(":/images/actions/go-home.svg"), "", this); + setHome->setStyleSheet(buttonStyle); + setHome->setToolTip(tr("Set home")); + setHome->setStatusTip(tr("Set home")); + zoomin->setMaximumWidth(30); zoomout->setMaximumWidth(30); createPath->setMaximumWidth(30); // clearTracking->setMaximumWidth(30); followgps->setMaximumWidth(30); + setHome->setMaximumWidth(30); goToButton->setMaximumWidth(30); // Set checkable buttons @@ -195,6 +203,7 @@ void MapWidget::init() // create a style and the slots to change the background so it is easier to distinguish followgps->setCheckable(true); createPath->setCheckable(true); + setHome->setCheckable(true); // add buttons to control the map (zoom, GPS tracking and WP capture) QGridLayout* innerlayout = new QGridLayout(mc); @@ -204,6 +213,7 @@ void MapWidget::init() innerlayout->addWidget(zoomout, 1, 0); innerlayout->addWidget(followgps, 2, 0); innerlayout->addWidget(createPath, 3, 0); + innerlayout->addWidget(setHome, 4, 0); //innerlayout->addWidget(clearTracking, 4, 0); // Add spacers to compress buttons on the top left innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); @@ -262,6 +272,12 @@ void MapWidget::init() connect(createPath, SIGNAL(clicked(bool)), this, SLOT(createPathButtonClicked(bool))); + connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePositionClick(bool))); + + connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*,QPointF)), this, + SLOT(createHomePosition(const QMouseEvent*,QPointF))); + //connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePosition(bool))); + connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)), this, SLOT(captureGeometryClick(Geometry*, QPoint))); @@ -1199,3 +1215,46 @@ QPointF MapWidget::getPointxBearing_Range(double lat1, double lon1, double beari return temp; } +void MapWidget::createHomePosition(const QMouseEvent *event, const QPointF coordinate) +{ + if (QEvent::MouseButtonRelease == event->type() && setHome->isChecked()) + { + homeCoordinate= coordinate; + Waypoint2DIcon* tempCirclePoint; + + double latitud = homeCoordinate.x(); + double longitud = homeCoordinate.y(); + + tempCirclePoint = new Waypoint2DIcon( + latitud, + longitud, + 20, "g", qmapcontrol::Point::Middle); + + QPen* pencil = new QPen(Qt::blue); + tempCirclePoint->setPen(pencil); + + mc->layer("Station")->clearGeometries(); + mc->layer("Station")->addGeometry(tempCirclePoint); + + qmapcontrol::Point* tempPoint = new qmapcontrol::Point(latitud, longitud,"g"); + + if (isVisible()) + { + mc->updateRequest(tempPoint->boundingBox().toRect()); + } + } +} + +void MapWidget::createHomePositionClick(bool click) +{ + Q_UNUSED(click); + + if (!setHome->isChecked()) + { + UASManager::instance()->setHomePosition( + static_cast(homeCoordinate.x()), + static_cast(homeCoordinate.y()), 0); + + qDebug()<<"Set home position "< - 0 + 1 @@ -184,70 +184,34 @@ - + - - - - - Elevator - - - - - - - Rudder - - - - - - - Throttle - - - - - - - Ailerons - - - - + + + Elevator + + + + + + + Rudder + + - - - - - Right Aileron - - - - - - - Right Elevator - - - - - - - Left Flap - - - - - - - Right Flap - - - - + + + Throttle + + + + + + + Ailerons + + diff --git a/src/ui/uas/UASControlParameters.cpp b/src/ui/uas/UASControlParameters.cpp index 6cf3414e0a5e346c4b16c612c0d9b067fd156cd8..08402b4e4a816f612ee7af095fb3a23f841f846d 100644 --- a/src/ui/uas/UASControlParameters.cpp +++ b/src/ui/uas/UASControlParameters.cpp @@ -27,6 +27,8 @@ UASControlParameters::UASControlParameters(QWidget *parent) : { ui->setupUi(this); + ui->btSetCtrl->setStatusTip(tr("Set Passthrough")); + connect(ui->btGetCommands, SIGNAL(clicked()), this, SLOT(getCommands())); connect(ui->btSetCtrl, SIGNAL(clicked()), this, SLOT(setPassthrough())); @@ -91,11 +93,8 @@ void UASControlParameters::activeUasSet(UASInterface *uas) connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) ); - //connect(uas, SIGNAL(radioCalibrationReceived(QPointer)), this, SLOT(radioChanged(QPointer))); activeUAS= uas; } @@ -122,16 +121,19 @@ void UASControlParameters::updateAttitude(UASInterface *uas, double roll, double void UASControlParameters::setCommands() { - UAS* myUas= static_cast(this->activeUAS); + if(this->activeUAS) + { + UAS* myUas= static_cast(this->activeUAS); - mavlink_message_t msg; + mavlink_message_t msg; - tempCmds.uCommand = ui->sbAirSpeed->value(); - tempCmds.hCommand = ui->sbHeight->value(); - tempCmds.rCommand = ui->sbTurnRate->value(); + tempCmds.uCommand = ui->sbAirSpeed->value(); + tempCmds.hCommand = ui->sbHeight->value(); + tempCmds.rCommand = ui->sbTurnRate->value(); - mavlink_msg_mid_lvl_cmds_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCmds); - myUas->sendMessage(msg); + mavlink_msg_mid_lvl_cmds_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCmds); + myUas->sendMessage(msg); + } } void UASControlParameters::getCommands() @@ -143,54 +145,43 @@ void UASControlParameters::getCommands() void UASControlParameters::setPassthrough() { - //UAS* myUas= static_cast(this->activeUAS); + if(this->activeUAS) + { + UAS* myUas= static_cast(this->activeUAS); - //mavlink_message_t msg; + mavlink_message_t msg; - int8_t tmpBit=0; + int8_t tmpBit=0; - if(ui->cxAilerons->isChecked()) - { - tmpBit+=1; - } - if(ui->cxElevator->isChecked()) - { - tmpBit+=2; - } - if(ui->cxLeftFlap->isChecked()) - { - tmpBit+=4; - } - if(ui->cxRightAileron->isChecked()) - { - tmpBit+=8; - } - if(ui->cxRightElevator->isChecked()) - { - tmpBit+=16; - } - if(ui->cxRightFlap->isChecked()) - { - tmpBit+=32; - } - if(ui->cxRudder->isChecked()) - { - tmpBit+=64; - } - if(ui->cxThrottle->isChecked()) - { - tmpBit+=128; - } + if(ui->cxdle_c->isChecked())//left elevator command + { + tmpBit+=8; + } + if(ui->cxdr_c->isChecked())//rudder command + { + tmpBit+=16; + } - generic_16bit r; - r.b[1] = 0; - r.b[0] = tmpBit;//255; + if(ui->cxdla_c->isChecked())//left aileron command + { + tmpBit+=64; + } + if(ui->cxdt_c->isChecked())//throttle command + { + tmpBit+=128; + } - tempCtrl.bitfieldPt= (uint16_t)r.s; + generic_16bit r; + r.b[1] = 0; + r.b[0] = tmpBit;//255; - //mavlink_msg_ctrl_srfc_pt_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCtrl); - //myUas->sendMessage(msg); - qDebug()<activeUAS->getUASID(); + tempCtrl.bitfieldPt= (uint16_t)r.s; + + mavlink_msg_ctrl_srfc_pt_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCtrl); + myUas->sendMessage(msg); + //qDebug()<throttle= throttle; } - - -void UASControlParameters::radioChanged(const QPointer& radio) -{ - if (radio) - { - if (this->radio) - { - delete this->radio; - } - - this->radio = new RadioCalibrationData(*radio); - -// aileron->set((*radio)(RadioCalibrationData::AILERON)); -// elevator->set((*radio)(RadioCalibrationData::ELEVATOR)); -// rudder->set((*radio)(RadioCalibrationData::RUDDER)); -// gyro->set((*radio)(RadioCalibrationData::GYRO)); -// pitch->set((*radio)(RadioCalibrationData::PITCH)); -// throttle->set((*radio)(RadioCalibrationData::THROTTLE)); - } -} diff --git a/src/ui/uas/UASControlParameters.h b/src/ui/uas/UASControlParameters.h index 7051cdbb1ac72c7b73001540744f8adab7ea90e5..c21f5e53dbc2e3b8bae88da086f97d673b88f1a5 100644 --- a/src/ui/uas/UASControlParameters.h +++ b/src/ui/uas/UASControlParameters.h @@ -31,7 +31,6 @@ public slots: void setPassthrough(); void updateMode(int uas,QString mode,QString description); void thrustChanged(UASInterface* uas,double throttle); - void radioChanged(const QPointer& radio); private: Ui::UASControlParameters *ui;