diff --git a/.gitignore b/.gitignore index 17b41c76c..a5af05849 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,9 @@ /flight/OpenPilot/Build /flight/OpenPilot/Build.win32 +#flight/Project/OpenPilotOSX +flight/Project/OpenPilotOSX/build + # /flight/PipBee/ /flight/PipBee/Build diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m new file mode 100644 index 000000000..39e89ffc1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro index 426579e77..2e4695328 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro +++ b/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) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui index 024db71bb..0d2889785 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui @@ -41,8 +41,21 @@ + + + + false + + + UDP Control + + + + + true + Armed diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 3c00987f7..b95082fb9 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/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(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(); UAVDataObject* obj = dynamic_cast( 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; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h index 4d465b0cd..216e6d075 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h @@ -34,6 +34,9 @@ #include "sdlgamepad/sdlgamepad.h" #include #include "gcscontrolplugin.h" +#include +#include + namespace Core { class IUAVGadget; @@ -59,6 +62,7 @@ public: private: ManualControlCommand* getManualControlCommand(); + double constrain(double value); QTime joystickTime; QWidget *m_widget; QList 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); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 24a72e62a..c213088c6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/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++) { diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h index c3ca4c6d3..8f1a07359 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h @@ -29,6 +29,7 @@ #define GCSCONTROLGADGETCONFIGURATION_H #include +#include 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 getChannelsMapping(); QList getChannelsReverse(); @@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration int throttleChannel; buttonSettingsStruct buttonSettings[8]; bool channelReverse[8]; + int udp_port; + QHostAddress udp_host; }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index ad0bf9a34..1a68c6be6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/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); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui index 260aa4e04..5191788a0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -141,7 +141,7 @@ - + 0 @@ -1011,6 +1011,66 @@ + + + UDP Setup + + + + + 20 + 20 + 301 + 71 + + + + UDP Port Configuration + + + + + + Host: + + + + + + + + 120 + 16777215 + + + + 127.0.0.1 + + + + + + + Port: + + + + + + + + 50 + 16777215 + + + + 2323 + + + + + + diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 6ee123ff6..09e25132d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -34,6 +34,7 @@ #include #include + #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(); +} /** diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index 58c2ecb0f..5f4b3eba0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -31,6 +31,8 @@ #include #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;