From 5fd4daa8f894f162f2b2371e3ff6443d39d4086b Mon Sep 17 00:00:00 2001 From: PT_Dreamer Date: Mon, 2 Jul 2012 16:11:34 +0100 Subject: [PATCH 01/10] GCS/Uploader - Some changes to fix "halt" on Linux --- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 2 +- ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp | 4 ++++ .../src/plugins/uploader/uploadergadgetwidget.cpp | 10 ++++++++-- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 03da96a6f..756dfbd4c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -812,7 +812,7 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset + length); // Send buffer, check that the transmit backlog does not grow above limit - if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) + if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE ) { io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH); } diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index bff5d04d5..dc08d94f2 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -90,6 +90,8 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): if (devices.length()==1 && hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { qDebug()<<"OP_DFU detected first time"; mready=true; + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + m_eventloop.exec(); } else { // Wait for the board to appear on the USB bus: USBSignalFilter filter(0x20a0,-1,-1,USBMonitor::Bootloader); @@ -107,6 +109,8 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): if (devices.length()==1) { if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + m_eventloop.exec(); qDebug()<<"OP_DFU detected after delay"; mready=true; break; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index a1008306c..a692433b1 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -213,7 +213,8 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->haltButton->setEnabled(true); break; } - delay::msleep(600); + QTimer::singleShot(600, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); fwIAP->getField("Command")->setValue("2233"); currentStep = IAP_STATE_STEP_2; log(QString("IAP Step 2")); @@ -228,7 +229,8 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->haltButton->setEnabled(true); break; } - delay::msleep(600); + QTimer::singleShot(600, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); fwIAP->getField("Command")->setValue("3344"); currentStep = IAP_STEP_RESET; log(QString("IAP Step 3")); @@ -250,8 +252,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) QString dli = cm->getCurrentDevice().Name; QString dlj = cm->getCurrentDevice().devName; cm->disconnectDevice(); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); // Tell connections to stop their polling threads: otherwise it will mess up DFU cm->suspendPolling(); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); log("Board Halt"); m_config->boardStatus->setText("Bootloader"); if (dlj.startsWith("USB")) From 622ea04a1fdd79ca7d48b4f5aecac6be92ef38fe Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Mon, 9 Jul 2012 18:09:57 +0200 Subject: [PATCH 02/10] Changed .gitignore the ignore build files from XCode project. --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) 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 From 4df81c5f852af3a836829f117c8c81b2d95e2f2c Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Mon, 9 Jul 2012 18:18:57 +0200 Subject: [PATCH 03/10] UDP Control Plugin. Initial merge for review. --- flight/Revolution/System/pios_board.c | 6 +- .../plugins/gcscontrol/Matlab/GCSControl.m | 29 +++++ .../src/plugins/gcscontrol/gcscontrol.pro | 1 + .../src/plugins/gcscontrol/gcscontrol.ui | 13 +++ .../plugins/gcscontrol/gcscontrolgadget.cpp | 107 +++++++++++++++++- .../src/plugins/gcscontrol/gcscontrolgadget.h | 7 ++ .../gcscontrolgadgetconfiguration.cpp | 24 ++++ .../gcscontrolgadgetconfiguration.h | 11 ++ .../gcscontrolgadgetoptionspage.cpp | 11 +- .../gcscontrol/gcscontrolgadgetoptionspage.ui | 68 ++++++++++- .../gcscontrol/gcscontrolgadgetwidget.cpp | 33 +++++- .../gcscontrol/gcscontrolgadgetwidget.h | 5 + 12 files changed, 305 insertions(+), 10 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 705463186..6e2319134 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -31,7 +31,7 @@ #include #include -#include "hwsettings.h" +#include #include "manualcontrolsettings.h" #include "board_hw_defs.c" @@ -483,8 +483,8 @@ void PIOS_Board_Init(void) { case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); 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..218a0b78b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m @@ -0,0 +1,29 @@ +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..f5ddbdc94 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; @@ -159,6 +169,7 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r newThrottle = leftY; break; } + newThrottle = (newThrottle + 1)/2.0; //check if buttons have control over this axis... if so don't update it int buttonRollControl=0; @@ -174,7 +185,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 +203,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 +299,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 +368,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..969b4556b 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").toInt(); + 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..1f2c50e18 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -141,9 +141,9 @@ - + - 0 + 1 @@ -1011,6 +1011,70 @@ + + + UDP Setup + + + + + 20 + 20 + 191 + 121 + + + + UDP Port Configuration + + + + + 60 + 30 + 113 + 30 + + + + + + + 10 + 40 + 67 + 17 + + + + Port: + + + + + + 60 + 70 + 113 + 30 + + + + + + + 10 + 80 + 67 + 17 + + + + Host: + + + + 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; From 8c252f4474bd5f949101c68495627296e01fac5d Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Thu, 12 Jul 2012 14:39:36 +0200 Subject: [PATCH 04/10] Fixed bug in throttle. Slightly reworked UDP config page. Added comments to matlab file. --- .../plugins/gcscontrol/Matlab/GCSControl.m | 19 ++++ .../plugins/gcscontrol/gcscontrolgadget.cpp | 1 - .../gcscontrol/gcscontrolgadgetoptionspage.ui | 94 +++++++++---------- 3 files changed, 64 insertions(+), 50 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m index 218a0b78b..39e89ffc1 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m +++ b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m @@ -1,3 +1,22 @@ +% 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 diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index f5ddbdc94..b95082fb9 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -169,7 +169,6 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r newThrottle = leftY; break; } - newThrottle = (newThrottle + 1)/2.0; //check if buttons have control over this axis... if so don't update it int buttonRollControl=0; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui index 1f2c50e18..5191788a0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -143,7 +143,7 @@ - 1 + 0 @@ -1020,59 +1020,55 @@ 20 20 - 191 - 121 + 301 + 71 UDP Port Configuration - - - - 60 - 30 - 113 - 30 - - - - - - - 10 - 40 - 67 - 17 - - - - Port: - - - - - - 60 - 70 - 113 - 30 - - - - - - - 10 - 80 - 67 - 17 - - - - Host: - - + + + + + Host: + + + + + + + + 120 + 16777215 + + + + 127.0.0.1 + + + + + + + Port: + + + + + + + + 50 + 16777215 + + + + 2323 + + + + From bc87319a0382aacd1bfb8b3b1b1cb2b929345aec Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Thu, 12 Jul 2012 14:41:41 +0200 Subject: [PATCH 05/10] Minor int vs. unsigned int bugfix. --- .../src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 969b4556b..c213088c6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp @@ -54,7 +54,7 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS yawChannel = qSettings->value("yawChannel").toInt(); throttleChannel = qSettings->value("throttleChannel").toInt(); - udp_port = qSettings->value("controlPortUDP").toInt(); + udp_port = qSettings->value("controlPortUDP").toUInt(); udp_host = QHostAddress(qSettings->value("controlHostUDP").toString()); int i; From 0efb24a54bd120104e12db77928a7fa4fcb5295e Mon Sep 17 00:00:00 2001 From: Michael Hope Date: Fri, 22 Jun 2012 20:22:43 +1200 Subject: [PATCH 06/10] The destination register of an exclusive store must be different from both the value and address register. Fix building with recent binutils by marking the result as early clobber. --- flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c index 56fddc52b..0e8c3c43c 100755 --- a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c +++ b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c @@ -733,7 +733,7 @@ uint32_t __STREXB(uint8_t value, uint8_t *addr) { uint32_t result=0; - __ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strexb %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) ); return(result); } @@ -750,7 +750,7 @@ uint32_t __STREXH(uint16_t value, uint16_t *addr) { uint32_t result=0; - __ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strexh %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) ); return(result); } From fa3aa20e058ff91adeb1c39b89f2ae004361ec81 Mon Sep 17 00:00:00 2001 From: Michael Hope Date: Fri, 22 Jun 2012 20:21:56 +1200 Subject: [PATCH 07/10] ld: make sure current pointer never goes backwards Fixes linking on ld version 2.22. Identical to 3cbf4499. --- .../STM32F10x/link_STM32103CB_PIPXTREME_sections.ld | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld index 568dddffa..074dad236 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld @@ -98,16 +98,9 @@ SECTIONS _init_stack_top = . - 4 ; } > SRAM + _eram = ORIGIN(SRAM) + LENGTH(SRAM) ; + _ebss = _eram ; - _free_ram = . ; - .free_ram (NOLOAD) : - { - . = ORIGIN(SRAM) + LENGTH(SRAM) - _free_ram ; - /* This is used by the startup in order to initialize the .bss section */ - _ebss = . ; - _eram = . ; - } > SRAM - /* keep the heap section at the end of the SRAM * this will allow to claim the remaining bytes not used * at run time! (done by the reset vector). From ce6bebceb69f6bb1367e1b2c3c7b9fcad8b9f6c6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 14 Jul 2012 19:36:07 -0500 Subject: [PATCH 08/10] Also strex needs to not clobber it's register to have predictable results. OPReview-227 --- flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c index 0e8c3c43c..d202e369b 100755 --- a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c +++ b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c @@ -767,7 +767,7 @@ uint32_t __STREXW(uint32_t value, uint32_t *addr) { uint32_t result=0; - __ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) ); + __ASM volatile ("strex %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) ); return(result); } From 9f67e24ee582a66230e683f40b1b843446f7041f Mon Sep 17 00:00:00 2001 From: Mike LaBranche Date: Mon, 16 Jul 2012 13:08:12 -0700 Subject: [PATCH 09/10] MultiRotor Config, Bugfix: include TriYaw channel in getChannelDescriptions --- .../plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index d27f4c530..bfa4fe7a2 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -274,6 +274,8 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() channelDesc[multi.VTOLMotorW-1] = QString("VTOLMotorW"); if (multi.VTOLMotorE > 0 && multi.VTOLMotorE < ConfigMultiRotorWidget::CHANNEL_NUMELEM) channelDesc[multi.VTOLMotorE-1] = QString("VTOLMotorE"); + if (multi.TRIYaw > 0 && multi.TRIYaw <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) + channelDesc[multi.TRIYaw-1] = QString("Tri-Yaw"); return channelDesc; } From 3ad711142392f83596548997a28641457898c8ac Mon Sep 17 00:00:00 2001 From: Laura Sebesta Date: Tue, 17 Jul 2012 23:06:47 +0200 Subject: [PATCH 10/10] Reverted change in pios_board.c, relating to revo udp control. Original commit had unnecessary changes. --- flight/Revolution/System/pios_board.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 6e2319134..705463186 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -31,7 +31,7 @@ #include #include -#include +#include "hwsettings.h" #include "manualcontrolsettings.h" #include "board_hw_defs.c" @@ -483,8 +483,8 @@ void PIOS_Board_Init(void) { case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);