rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

UDP Control Plugin. Initial merge for review.

Browse Source
This commit is contained in:
Kenz Dale 2012-07-09 18:18:57 +02:00
parent 622ea04a1f
commit 4df81c5f85
12 changed files with 305 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/Revolution/System/pios_board.c
View File

@ -31,7 +31,7 @@
#include <openpilot.h>
#include <uavobjectsinit.h>
#include "hwsettings.h"
#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_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);

29
ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m Normal file
View File

@ -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

1
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro
View File

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

13
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
View File

@ -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>

107
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
View File

@ -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;
@ -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<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 +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;

7
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h
View File

@ -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);

24
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp
View File

@ -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++)
{

11
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h
View File

@ -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;
};

11
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp
View File

@ -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);

68
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui
View File

@ -141,9 +141,9 @@
</widget>
</item>
<item>
<widget class="QTabWidget" name="tabWidget">
<widget class="QTabWidget" name="UDPSetup">
<property name="currentIndex">
<number>0</number>
<number>1</number>
</property>
<widget class="QWidget" name="tabWidgetPage2">
<attribute name="title">
@ -1011,6 +1011,70 @@
</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>191</width>
<height>121</height>
</rect>
</property>
<property name="title">
<string>UDP Port Configuration</string>
</property>
<widget class="QLineEdit" name="udp_port">
<property name="geometry">
<rect>
<x>60</x>
<y>30</y>
<width>113</width>
<height>30</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_6">
<property name="geometry">
<rect>
<x>10</x>
<y>40</y>
<width>67</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Port:</string>
</property>
</widget>
<widget class="QLineEdit" name="udp_host">
<property name="geometry">
<rect>
<x>60</x>
<y>70</y>
<width>113</width>
<height>30</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_7">
<property name="geometry">
<rect>
<x>10</x>
<y>80</y>
<width>67</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Host:</string>
</property>
</widget>
</widget>
</widget>
</widget>
</item>
</layout>

33
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
View File

@ -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();
}
/**

5
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
View File

@ -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;