diff --git a/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp index 7e4c68173..1a375cce3 100644 --- a/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -30,11 +30,17 @@ #include "extensionsystem/pluginmanager.h" #include "uavobjects/uavobjectmanager.h" #include "uavobjects/uavobject.h" +#include GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent) : IUAVGadget(classId, parent), m_widget(widget) { + connect(getManualControlCommand(),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(manualControlCommandUpdated(UAVObject*))); + connect(widget,SIGNAL(sticksChanged(double,double,double,double)),this,SLOT(sticksChangedLocally(double,double,double,double))); + connect(this,SIGNAL(sticksChangedRemotely(double,double,double,double)),widget,SLOT(updateSticks(double,double,double,double))); + + manualControlCommandUpdated(getManualControlCommand()); } GCSControlGadget::~GCSControlGadget() @@ -49,3 +55,26 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); } + +ManualControlCommand* GCSControlGadget::getManualControlCommand() { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + return dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); +} + +void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { + double roll = obj->getField("Roll")->getDouble(); + double pitch = obj->getField("Pitch")->getDouble(); + double yaw = obj->getField("Yaw")->getDouble(); + double throttle = obj->getField("Throttle")->getDouble(); + emit sticksChangedRemotely(yaw,-pitch,roll,throttle); +} + +void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY) { + ManualControlCommand * obj = getManualControlCommand(); + obj->getField("Roll")->setDouble(rightX); + obj->getField("Pitch")->setDouble(-leftY); + obj->getField("Yaw")->setDouble(leftX); + obj->getField("Throttle")->setDouble(rightY); + obj->updated(); +} diff --git a/ground/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/src/plugins/gcscontrol/gcscontrolgadget.h index 0d8b7551c..734a91af9 100644 --- a/ground/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/src/plugins/gcscontrol/gcscontrolgadget.h @@ -29,6 +29,7 @@ #define GCSControlGADGET_H_ #include +#include namespace Core { class IUAVGadget; @@ -51,9 +52,20 @@ public: QString contextHelpId() const { return QString(); } void loadConfiguration(IUAVGadgetConfiguration* config); + private: - QWidget *m_widget; - QList m_context; + ManualControlCommand* getManualControlCommand(); + + QWidget *m_widget; + QList m_context; + UAVObject::Metadata mccInitialData; + +signals: + void sticksChangedRemotely(double leftX, double leftY, double rightX, double rightY); + +protected slots: + void manualControlCommandUpdated(UAVObject *); + void sticksChangedLocally(double leftX, double leftY, double rightX, double rightY); }; diff --git a/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 7026f82a0..669c000f5 100644 --- a/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -44,19 +44,33 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) m_gcscontrol = new Ui_GCSControl(); m_gcscontrol->setupUi(this); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVObject::Metadata mdata = obj->getMetadata(); + m_gcscontrol->checkBoxGcsControl->setChecked(mdata.flightAccess == UAVObject::ACCESS_READONLY); + // Set up the drop down box for the flightmode - m_gcscontrol->comboBoxFlightMode->addItem(QString("Manual")); - m_gcscontrol->comboBoxFlightMode->addItem(QString("Stabilized")); - m_gcscontrol->comboBoxFlightMode->addItem(QString("Auto")); + m_gcscontrol->comboBoxFlightMode->addItems(obj->getField("FlightMode")->getOptions()); - // Set up slots and signals - connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(gcsControlToggle(int))); - connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(flightModeChanged(int))); - //connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(gcsArmedToggle(int))); + // Set up slots and signals for joysticks + connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double))); + connect(m_gcscontrol->widgetRightStick,SIGNAL(positionClicked(double,double)),this,SLOT(rightStickClicked(double,double))); - // Connect object updated event from UAVObject to also update check boxes - connect(getMCC(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); + // Connect misc controls + connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int))); + connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int))); + connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int))); + // Connect object updated event from UAVObject to also update check boxes and dropdown + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); + + + leftX = 0; + leftY = 0; + rightX = 0; + rightY = 0; } GCSControlGadgetWidget::~GCSControlGadgetWidget() @@ -64,28 +78,33 @@ GCSControlGadgetWidget::~GCSControlGadgetWidget() // Do nothing } -/*! - \brief Returns the ManualControlCommand UAVObject - */ -ManualControlCommand* GCSControlGadgetWidget::getMCC() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - ManualControlCommand* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); - return obj; +void GCSControlGadgetWidget::updateSticks(double leftX, double leftY, double rightX, double rightY) { + m_gcscontrol->widgetLeftStick->changePosition(leftX,leftY); + m_gcscontrol->widgetRightStick->changePosition(rightX,rightY); } -void GCSControlGadgetWidget::mccChanged(UAVObject* obj) -{ - m_gcscontrol->checkBoxArmed->setChecked(obj->getField("Armed")->getValue().toString() == "True"); +void GCSControlGadgetWidget::leftStickClicked(double X, double Y) { + leftX = X; + leftY = Y; + emit sticksChanged(leftX,leftY,rightX,rightY); +} + +void GCSControlGadgetWidget::rightStickClicked(double X, double Y) { + rightX = X; + rightY = Y; + emit sticksChanged(leftX,leftY,rightX,rightY); } /*! \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command */ -void GCSControlGadgetWidget::gcsControlToggle(int state) +void GCSControlGadgetWidget::toggleControl(int state) { - UAVObject::Metadata mdata = getMCC()->getMetadata(); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + + UAVObject::Metadata mdata = obj->getMetadata(); if (state) { mccInitialData = mdata; @@ -100,28 +119,38 @@ void GCSControlGadgetWidget::gcsControlToggle(int state) { mdata = mccInitialData; } - getMCC()->setMetadata(mdata); + obj->setMetadata(mdata); +} + +void GCSControlGadgetWidget::toggleArmed(int state) +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + if(state) + obj->getField("Armed")->setValue("True"); + else + obj->getField("Armed")->setValue("False"); + obj->updated(); +} + +void GCSControlGadgetWidget::mccChanged(UAVObject * obj) +{ + m_gcscontrol->checkBoxArmed->setChecked(obj->getField("Armed")->getValue() == "True"); + m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(obj->getField("FlightMode")->getValue().toString())); } /*! \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly */ -void GCSControlGadgetWidget::flightModeChanged(int state) +void GCSControlGadgetWidget::selectFlightMode(int state) { - ManualControlCommand::DataFields data = getMCC()->getData(); - if( state == 0 ) - { - data.FlightMode = ManualControlCommand::FLIGHTMODE_MANUAL; - } - else if ( state == 1 ) - { - data.FlightMode = ManualControlCommand::FLIGHTMODE_STABILIZED; - } - else if ( state == 2 ) - { - data.FlightMode = ManualControlCommand::FLIGHTMODE_AUTO; - } - getMCC()->setData(data); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); + UAVObjectField * field = obj->getField("FlightMode"); + field->setValue(field->getOptions()[state]); + obj->updated(); } /** diff --git a/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index b2a16fb4c..6bd54a8d7 100644 --- a/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -39,19 +39,29 @@ class GCSControlGadgetWidget : public QLabel public: GCSControlGadgetWidget(QWidget *parent = 0); - ~GCSControlGadgetWidget(); + ~GCSControlGadgetWidget(); -private slots: - void gcsControlToggle(int state); - void flightModeChanged(int state); +signals: + void sticksChanged(double leftX, double leftY, double rightX, double rightY); + +public slots: + // signals from parent gadget indicating change from flight + void updateSticks(double leftX, double leftY, double rightX, double rightY); + + // signals from children widgets indicating a local change + void leftStickClicked(double X, double Y); + void rightStickClicked(double X, double Y); protected slots: - void mccChanged(UAVObject*); + void toggleControl(int state); + void toggleArmed(int state); + void selectFlightMode(int state); + void mccChanged(UAVObject *); private: - Ui_GCSControl *m_gcscontrol; - ManualControlCommand* getMCC(); - UAVObject::Metadata mccInitialData; + Ui_GCSControl *m_gcscontrol; + UAVObject::Metadata mccInitialData; + double leftX,leftY,rightX,rightY; }; #endif /* GCSControlGADGETWIDGET_H_ */ diff --git a/ground/src/plugins/gcscontrol/joystickcontrol.cpp b/ground/src/plugins/gcscontrol/joystickcontrol.cpp index 6e9d1a783..cd432ee2a 100644 --- a/ground/src/plugins/gcscontrol/joystickcontrol.cpp +++ b/ground/src/plugins/gcscontrol/joystickcontrol.cpp @@ -48,10 +48,6 @@ JoystickControl::JoystickControl(QWidget *parent) : setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing); - // Connect object updated event from UAVObject to also animate sticks - connect(getMCC(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); - - m_renderer = new QSvgRenderer(); bool test = m_renderer->load(QString(":/gcscontrol/images/joystick.svg")); Q_ASSERT( test ); @@ -78,59 +74,38 @@ JoystickControl::~JoystickControl() } /** - * @brief Draw stick positions when ManualControlCommand gets updated + * @brief Update the displayed position based on an MCC update */ -void JoystickControl::mccChanged(UAVObject*) +void JoystickControl::changePosition(double X, double Y) { - QPen pen; - pen.setColor(QColor(Qt::white)); - pen.setWidth(2); - - // draw stick positions - if( this->objectName() == QString("widgetLeftStick")) - { - ManualControlCommand::DataFields data = getMCC()->getData(); - double x = (data.Yaw + 1) / 2 * scene()->sceneRect().width(); - double y = (data.Pitch + 1) / 2 * scene()->sceneRect().height(); - m_joystickEnd->setPos(x-m_joystickEnd->boundingRect().width()/2,y-m_joystickEnd->boundingRect().height()/2); - } - else if( this->objectName() == QString("widgetRightStick")) - { - ManualControlCommand::DataFields data = getMCC()->getData(); - double x = (data.Roll + 1) / 2 * scene()->sceneRect().width(); - double y = data.Throttle * scene()->sceneRect().height(); - m_joystickEnd->setPos(x-m_joystickEnd->boundingRect().width()/2,scene()->sceneRect().height()-y - 200/scene()->sceneRect().height()); - } + QRectF sceneSize = scene()->sceneRect(); + m_joystickEnd->setPos((X+1)/2*sceneSize.width(),(-Y+1)/2*sceneSize.height()); } /** - * @brief Get the Manual Control Command UAV Object - */ -ManualControlCommand* JoystickControl::getMCC() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - ManualControlCommand* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); - return obj; -} - -/** - * @brief Redirect mouse move events to control joystick position + * @brief Redirect mouse move events to control position */ void JoystickControl::mouseMoveEvent(QMouseEvent *event) { - updateMCC(mapToScene(event->pos())); + QPointF point = mapToScene(event->pos()); + QRectF sceneSize = scene()->sceneRect(); + + double Y = - (point.y() / sceneSize.height() - .5) * 2; + double X = (point.x() / sceneSize.width() - .5) * 2; + emit positionClicked(X, Y); } /** - * @brief Redirect mouse move clicks to control joystick position + * @brief Redirect mouse move clicks to control position */ void JoystickControl::mousePressEvent(QMouseEvent *event) { - if( event->button() == Qt::LeftButton ) - updateMCC(mapToScene(event->pos())); + if( event->button() == Qt::LeftButton ) { + mouseMoveEvent(event); + } } + void JoystickControl::paint() { update(); @@ -140,8 +115,7 @@ void JoystickControl::paintEvent(QPaintEvent *event) { // Skip painting until the dial file is loaded if (! m_renderer->isValid()) { - qDebug()<<"Dial file not loaded, not rendering"; -// return; + qDebug()<<"Image file not loaded, not rendering"; } QGraphicsView::paintEvent(event); @@ -149,40 +123,10 @@ void JoystickControl::paintEvent(QPaintEvent *event) void JoystickControl::resizeEvent(QResizeEvent *event) { - fitInView(m_background, Qt::KeepAspectRatio ); + Q_UNUSED(event); + fitInView(m_background, Qt::IgnoreAspectRatio ); } -/** - * @brief Update the roll,pitch,yaw,throttle for the Manual Control Command based on stick position (from mouse event) - */ -void JoystickControl::updateMCC(QPointF point) -{ - QRectF sceneSize = scene()->sceneRect(); - - double x = 2 * ( point.x() / sceneSize.width() - .5 ); - x = qBound( (double) -1, x, (double) 1); - if( this->objectName() == QString("widgetLeftStick")) - { - double y = 2 * ( point.y() / sceneSize.height() - .5); - y = qBound( (double) -1, y, (double) 1); - - ManualControlCommand::DataFields data = getMCC()->getData(); - data.Pitch = y; - data.Yaw = x; - getMCC()->setData(data); - } - - if( this->objectName() == QString("widgetRightStick")) - { - double y = 1-( point.y() / sceneSize.height()); - y = qBound( (double) 0, y, (double) 1); - - ManualControlCommand::DataFields data = getMCC()->getData(); - data.Throttle = y; - data.Roll = x; - getMCC()->setData(data); - } -} /** * @} diff --git a/ground/src/plugins/gcscontrol/joystickcontrol.h b/ground/src/plugins/gcscontrol/joystickcontrol.h index 43a63c7fb..a4cd53034 100644 --- a/ground/src/plugins/gcscontrol/joystickcontrol.h +++ b/ground/src/plugins/gcscontrol/joystickcontrol.h @@ -54,11 +54,11 @@ protected: void paintEvent(QPaintEvent *event); void resizeEvent(QResizeEvent *event); - void updateMCC(QPointF); - ManualControlCommand* getMCC(); +public slots: + void changePosition (double X, double Y); - protected slots: - void mccChanged(UAVObject*); +signals: + void positionClicked(double X, double Y); private: QSvgRenderer *m_renderer;