Ground/Joystick Control: Separated the logic for changing values from the

visualization to prepare for real joystick support.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1785 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/plugins/gcscontrol/gcscontrolgadget.cpp

@@ -30,11 +30,17 @@
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjects/uavobjectmanager.h"
 #include "uavobjects/uavobject.h"
+#include <QDebug>
 
 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<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
 }
+
+ManualControlCommand* GCSControlGadget::getManualControlCommand() {
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
+    return dynamic_cast<ManualControlCommand*>( 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();
+}

ground/src/plugins/gcscontrol/gcscontrolgadget.h

@@ -29,6 +29,7 @@
 #define GCSControlGADGET_H_
 
 #include <coreplugin/iuavgadget.h>
+#include <uavobjects/manualcontrolcommand.h>
 
 namespace Core {
 class IUAVGadget;
@@ -51,9 +52,20 @@ public:
     QString contextHelpId() const { return QString(); }
 
     void loadConfiguration(IUAVGadgetConfiguration* config);
+
 private:
-        QWidget *m_widget;
-	QList<int> m_context;
+    ManualControlCommand* getManualControlCommand();
+
+    QWidget *m_widget;
+    QList<int> 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);
 };
 
 

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<UAVObjectManager>();
+    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( 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<UAVObjectManager>();
-    ManualControlCommand* obj = dynamic_cast<ManualControlCommand*>( 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<UAVObjectManager>();
+    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( 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<UAVObjectManager>();
+    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( 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<UAVObjectManager>();
+    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
+    UAVObjectField * field = obj->getField("FlightMode");
+    field->setValue(field->getOptions()[state]);
+    obj->updated();
 }
 
 /**

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_ */

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<UAVObjectManager>();
-    ManualControlCommand* obj = dynamic_cast<ManualControlCommand*>( 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);
-    }
-}
 
 /**
   * @}

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;