Merge remote-tracking branch 'origin/rghunter/udp_control' into next

.gitignore

@@ -20,6 +20,9 @@
 /flight/OpenPilot/Build
 /flight/OpenPilot/Build.win32
 
+#flight/Project/OpenPilotOSX
+flight/Project/OpenPilotOSX/build
+
 # /flight/PipBee/
 /flight/PipBee/Build
 

ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m

@@ -0,0 +1,48 @@
+% GCSCONTROL
+%  This class allows the user to send 4-axis stick commands to OpenPilot
+%  GCS.
+%
+% Create class by
+%    control = GCSControl
+%
+% Open connection by 
+%    control.connect('01.23.45.67', 89)
+% where the first value is the IP address of the computer running GCS and
+% the second value is the port on which GCS is listening.
+%
+% Send command by
+%    control.command(pitch, yaw, roll, throttle)
+% where all variables are between [-1,1]
+%
+% Close connection by
+%    control.close()
+
+classdef GCSControl < handle
+    
+    properties
+        udpObj;
+        isConnected=false;
+    end
+    
+    methods
+        function obj=GCSControl()
+            obj.isConnected = false;
+        end
+        function obj=connect(obj,rhost,rport)
+            obj.udpObj = udp(rhost,rport);
+            fopen(obj.udpObj);
+            obj.isConnected = true;
+        end
+        function obj=command(obj,pitch,yaw,roll,throttle)
+            if(obj.isConnected)
+                fwrite(obj.udpObj,[42,pitch,yaw,roll,throttle,36],'double')
+            end
+        end
+        function obj=close(obj)
+            if(obj.isConnected)
+                fclose(obj.udpObj);
+                obj.isConnected = false;
+            end
+        end
+    end
+end

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro

@@ -2,6 +2,7 @@ TEMPLATE = lib
 TARGET = GCSControl 
 QT += svg
 QT += opengl
+QT += network
 
 include(../../openpilotgcsplugin.pri) 
 include(../../plugins/coreplugin/coreplugin.pri) 

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui

@@ -41,8 +41,21 @@
        </property>
       </widget>
      </item>
+     <item>
+      <widget class="QCheckBox" name="checkBoxUDPControl">
+       <property name="enabled">
+        <bool>false</bool>
+       </property>
+       <property name="text">
+        <string>UDP Control</string>
+       </property>
+      </widget>
+     </item>
      <item>
       <widget class="QCheckBox" name="checkBoxArmed">
+       <property name="enabled">
+        <bool>true</bool>
+       </property>
        <property name="text">
         <string>Armed</string>
        </property>

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp

@@ -44,6 +44,10 @@ GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widg
 
     manualControlCommandUpdated(getManualControlCommand());
 
+    control_sock = new QUdpSocket(this);
+
+    connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand()));
+
     joystickTime.start();
     GCSControlPlugin *pl = dynamic_cast<GCSControlPlugin*>(plugin);
     connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8)));
@@ -67,6 +71,12 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config)
     yawChannel = ql.at(2);
     throttleChannel = ql.at(3);
 
+ //   if(control_sock->isOpen())
+ //       control_sock->close();
+    control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress);
+
+
+
     controlsMode = GCSControlConfig->getControlsMode();
 
     int i;
@@ -174,7 +184,8 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
     }
 
     //if we are not in local gcs control mode, ignore the joystick input
-    if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false)return;
+    if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
+        return;
 
     if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
         if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll);
@@ -191,6 +202,93 @@ void GCSControlGadget::gamepads(quint8 count)
 //    sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE);
 }
 
+void GCSControlGadget::readUDPCommand()
+{
+    double pitch, yaw, roll, throttle;
+    while (control_sock->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(control_sock->pendingDatagramSize());
+        control_sock->readDatagram(datagram.data(), datagram.size());
+        QDataStream readData(datagram);
+        bool badPack = false;
+        int state = 0;
+        while(!readData.atEnd() && !badPack)
+        {
+            double buffer;
+            readData >> buffer;
+            switch(state)
+            {
+            case 0:
+                if(buffer == 42){
+                    state = 1;
+                }else{
+                    state = 0;
+                    badPack = true;
+                }
+                break;
+            case 1:
+                pitch = buffer;
+                state = 2;
+                break;
+            case 2:
+                yaw = buffer;
+                state = 3;
+                break;
+            case 3:
+                roll = buffer;
+                state = 4;
+                break;
+            case 4:
+                throttle = buffer;
+                state = 5;
+                break;
+            case 5:
+                if(buffer != 36 || !readData.atEnd())
+                    badPack=true;
+                break;
+            }
+
+        }
+        if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
+        {
+             ManualControlCommand * obj = getManualControlCommand();
+             bool update = false;
+
+             if(pitch != obj->getField("Pitch")->getDouble()){
+                 obj->getField("Pitch")->setDouble(constrain(pitch));
+                 update = true;
+             }
+             if(yaw != obj->getField("Yaw")->getDouble()){
+                 obj->getField("Yaw")->setDouble(constrain(yaw));
+                 update = true;
+             }
+             if(roll != obj->getField("Roll")->getDouble()){
+                 obj->getField("Roll")->setDouble(constrain(roll));
+                 update = true;
+             }
+             if(throttle != obj->getField("Throttle")->getDouble()){
+                obj->getField("Throttle")->setDouble(constrain(throttle));
+                update = true;
+             }
+             if(update)
+                 obj->updated();
+        }
+    }
+
+    qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle;
+
+
+}
+
+double GCSControlGadget::constrain(double value)
+{
+    if(value < -1)
+        return -1;
+    if(value > 1)
+        return 1;
+    return value;
+}
+
 void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
 {
     int state;
@@ -200,6 +298,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
         UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
         UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
         bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
+        bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
 
         switch (buttonSettings[number].ActionID)
         {
@@ -268,6 +367,11 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
                 ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl);
 
                break;
+            case 3: //UDP Control
+                    if(currentCGSControl)
+                        ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl);
+
+                    break;
             }
 
             break;

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h

@@ -34,6 +34,9 @@
 #include "sdlgamepad/sdlgamepad.h"
 #include <QTime>
 #include "gcscontrolplugin.h"
+#include <QUdpSocket>
+#include <QHostAddress>
+
 
 namespace Core {
 class IUAVGadget;
@@ -59,6 +62,7 @@ public:
 
 private:
     ManualControlCommand* getManualControlCommand();
+    double constrain(double value);
     QTime joystickTime;
     QWidget *m_widget;
     QList<int> m_context;
@@ -72,6 +76,8 @@ private:
     double bound(double input);
     double wrap(double input);
     bool channelReverse[8];
+    QUdpSocket *control_sock;
+
 
 signals:
     void sticksChangedRemotely(double leftX, double leftY, double rightX, double rightY);
@@ -79,6 +85,7 @@ signals:
 protected slots:
     void manualControlCommandUpdated(UAVObject *);
     void sticksChangedLocally(double leftX, double leftY, double rightX, double rightY);
+    void readUDPCommand();
 
     // signals from joystick
     void gamepads(quint8 count);

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp

@@ -54,6 +54,9 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
         yawChannel = qSettings->value("yawChannel").toInt();
         throttleChannel = qSettings->value("throttleChannel").toInt();
 
+        udp_port = qSettings->value("controlPortUDP").toUInt();
+        udp_host = QHostAddress(qSettings->value("controlHostUDP").toString());
+
         int i;
         for (i=0;i<8;i++)
         {
@@ -66,6 +69,21 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
 
 }
 
+void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host)
+{
+    udp_port = port;
+    udp_host = QHostAddress(host);
+}
+
+int GCSControlGadgetConfiguration::getUDPControlPort()
+{
+    return udp_port;
+}
+QHostAddress GCSControlGadgetConfiguration::getUDPControlHost()
+{
+    return udp_host;
+}
+
 void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) {
     rollChannel = roll;
     pitchChannel = pitch;
@@ -102,6 +120,9 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone()
     m->yawChannel = yawChannel;
     m->throttleChannel = throttleChannel;
 
+    m->udp_host = udp_host;
+    m->udp_port = udp_port;
+
     int i;
     for (i=0;i<8;i++)
     {
@@ -126,6 +147,9 @@ void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const {
     settings->setValue("yawChannel", yawChannel);
     settings->setValue("throttleChannel", throttleChannel);
 
+    settings->setValue("controlPortUDP",QString::number(udp_port));
+    settings->setValue("controlHostUDP",udp_host.toString());
+
     int i;
     for (i=0;i<8;i++)
     {

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h

@@ -29,6 +29,7 @@
 #define GCSCONTROLGADGETCONFIGURATION_H
 
 #include <coreplugin/iuavgadgetconfiguration.h>
+#include <QtNetwork/QHostAddress>
 
 typedef struct{
     int     ActionID;
@@ -36,6 +37,11 @@ typedef struct{
     double  Amount;
 }buttonSettingsStruct;
 
+typedef struct{
+    int port;
+    QHostAddress address;
+}portSettingsStruct;
+
 
 using namespace Core;
 
@@ -49,6 +55,9 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
 
     void setControlsMode(int mode) { controlsMode = mode; }
     void setRPYTchannels(int roll, int pitch, int yaw, int throttle);
+    void setUDPControlSettings(int port, QString host);
+    int getUDPControlPort();
+    QHostAddress getUDPControlHost();
     int getControlsMode() { return controlsMode; }
     QList<int>  getChannelsMapping();
     QList<bool>  getChannelsReverse();
@@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
         int throttleChannel;
         buttonSettingsStruct buttonSettings[8];
         bool channelReverse[8];
+        int udp_port;
+        QHostAddress udp_host;
 
 
 };

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp

@@ -137,7 +137,7 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
               options_page->buttonFunction4 << options_page->buttonFunction5 <<
               options_page->buttonFunction6 << options_page->buttonFunction7;
     QStringList buttonOptions;
-    buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control" ;
+    buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list
     foreach (QComboBox* qb, buttonFunctionList) {
         qb->addItems(buttonOptions);
     }
@@ -187,6 +187,9 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
 
     //updateButtonFunction();
 
+    options_page->udp_host->setText(m_config->getUDPControlHost().toString());
+    options_page->udp_port->setText(QString::number(m_config->getUDPControlPort()));
+
 
     // Controls mode are from 1 to 4.
     if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5)
@@ -262,6 +265,9 @@ void GCSControlGadgetOptionsPage::apply()
    }
    m_config->setRPYTchannels(roll,pitch,yaw,throttle);
 
+   m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text());
+
+
    int j;
    for (j=0;j<8;j++)
    {
@@ -271,6 +277,7 @@ void GCSControlGadgetOptionsPage::apply()
        m_config->setChannelReverse(j,chRevList.at(j)->isChecked());
    }
 
+
 }
 
 void GCSControlGadgetOptionsPage::finish()
@@ -369,7 +376,7 @@ void GCSControlGadgetOptionsPage::updateButtonAction(int controlID)
         if (buttonActionList.at(i)->currentText().compare("Toggles")==0)
         {
             disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction()));
-            buttonOptions <<"-" << "Armed" << "GCS Control" ;
+            buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control";
             buttonFunctionList.at(i)->clear();
             buttonFunctionList.at(i)->insertItems(-1,buttonOptions);
 

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui

@@ -141,7 +141,7 @@
        </widget>
       </item>
       <item>
-       <widget class="QTabWidget" name="tabWidget">
+       <widget class="QTabWidget" name="UDPSetup">
         <property name="currentIndex">
          <number>0</number>
         </property>
@@ -1011,6 +1011,66 @@
           </item>
          </layout>
         </widget>
+        <widget class="QWidget" name="UDPSetup_2">
+         <attribute name="title">
+          <string>UDP Setup</string>
+         </attribute>
+         <widget class="QGroupBox" name="groupBox">
+          <property name="geometry">
+           <rect>
+            <x>20</x>
+            <y>20</y>
+            <width>301</width>
+            <height>71</height>
+           </rect>
+          </property>
+          <property name="title">
+           <string>UDP Port Configuration</string>
+          </property>
+          <layout class="QHBoxLayout" name="horizontalLayout_2">
+           <item>
+            <widget class="QLabel" name="label_7">
+             <property name="text">
+              <string>Host:</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLineEdit" name="udp_host">
+             <property name="maximumSize">
+              <size>
+               <width>120</width>
+               <height>16777215</height>
+              </size>
+             </property>
+             <property name="text">
+              <string>127.0.0.1</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLabel" name="label_6">
+             <property name="text">
+              <string>Port:</string>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="QLineEdit" name="udp_port">
+             <property name="maximumSize">
+              <size>
+               <width>50</width>
+               <height>16777215</height>
+              </size>
+             </property>
+             <property name="text">
+              <string>2323</string>
+             </property>
+            </widget>
+           </item>
+          </layout>
+         </widget>
+        </widget>
        </widget>
       </item>
      </layout>

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp

@@ -34,6 +34,7 @@
 #include <QtGui/QVBoxLayout>
 #include <QtGui/QPushButton>
 
+
 #include "uavobject.h"
 #include "uavobjectmanager.h"
 #include "manualcontrolcommand.h"
@@ -64,9 +65,14 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
     connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
     connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
 
+    connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox
+
     // 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;
@@ -122,11 +128,14 @@ void GCSControlGadgetWidget::toggleControl(int state)
         UAVObject::SetGcsTelemetryAcked(mdata, false);
         UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
         mdata.gcsTelemetryUpdatePeriod = 100;
+        m_gcscontrol->checkBoxUDPControl->setEnabled(true);
 
     }
     else
     {
         mdata = mccInitialData;
+        toggleUDPControl(false);
+        m_gcscontrol->checkBoxUDPControl->setEnabled(false);
     }
     obj->setMetadata(mdata);
 }
@@ -152,6 +161,16 @@ void GCSControlGadgetWidget::mccChanged(UAVObject * obj)
     m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
 }
 
+void GCSControlGadgetWidget::toggleUDPControl(int state)
+{
+    if(state)
+    {
+        setUDPControl(true);
+    }else{
+        setUDPControl(false);
+    }
+}
+
 /*!
   \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly
   */
@@ -168,11 +187,21 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
 void GCSControlGadgetWidget::setGCSControl(bool newState)
 {
     m_gcscontrol->checkBoxGcsControl->setChecked(newState);
-};
+}
 bool GCSControlGadgetWidget::getGCSControl(void)
 {
     return m_gcscontrol->checkBoxGcsControl->isChecked();
-};
+}
+
+void GCSControlGadgetWidget::setUDPControl(bool newState)
+{
+    m_gcscontrol->checkBoxUDPControl->setChecked(newState);
+}
+
+bool GCSControlGadgetWidget::getUDPControl(void)
+{
+    return m_gcscontrol->checkBoxUDPControl->isChecked();
+}
 
 
 /**

ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h

@@ -31,6 +31,8 @@
 #include <QtGui/QLabel>
 #include "manualcontrolcommand.h"
 
+#define UDP_PORT 2323
+
 class Ui_GCSControl;
 
 class GCSControlGadgetWidget : public QLabel
@@ -42,6 +44,8 @@ public:
     ~GCSControlGadgetWidget();
     void setGCSControl(bool newState);
     bool getGCSControl(void);
+    void setUDPControl(bool newState);
+    bool getUDPControl(void);
 
 signals:
     void sticksChanged(double leftX, double leftY, double rightX, double rightY);
@@ -59,6 +63,7 @@ protected slots:
     void toggleArmed(int state);
     void selectFlightMode(int state);
     void mccChanged(UAVObject *);
+    void toggleUDPControl(int state);
 
 private:
     Ui_GCSControl *m_gcscontrol;