rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

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

Browse Source
This commit is contained in:
James Cotton 2012-07-18 12:43:17 -05:00
commit be892e236d
12 changed files with 318 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

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

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

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

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>

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

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").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++)
{

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

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

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

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;