2011-02-02 00:01:52 +01:00
|
|
|
/**
|
|
|
|
******************************************************************************
|
|
|
|
*
|
|
|
|
* @file GCSControlgadget.cpp
|
|
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
|
|
* @addtogroup GCSPlugins GCS Plugins
|
|
|
|
* @{
|
|
|
|
* @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin
|
|
|
|
* @{
|
|
|
|
* @brief A gadget to control the UAV, either from the keyboard or a joystick
|
|
|
|
*****************************************************************************/
|
|
|
|
/*
|
|
|
|
* This program is free software; you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License as published by
|
|
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This program is distributed in the hope that it will be useful, but
|
|
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
|
|
* for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License along
|
|
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
*/
|
|
|
|
#include "gcscontrolgadget.h"
|
|
|
|
#include "gcscontrolgadgetwidget.h"
|
|
|
|
#include "gcscontrolgadgetconfiguration.h"
|
|
|
|
#include "extensionsystem/pluginmanager.h"
|
|
|
|
#include "uavobjectmanager.h"
|
|
|
|
#include "uavobject.h"
|
|
|
|
#include <QDebug>
|
|
|
|
|
|
|
|
#define JOYSTICK_UPDATE_RATE 50
|
|
|
|
|
|
|
|
GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent, QObject *plugin) :
|
2013-05-19 16:37:30 +02:00
|
|
|
IUAVGadget(classId, parent),
|
|
|
|
m_widget(widget)
|
2011-02-02 00:01:52 +01:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
connect(getManualControlCommand(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(manualControlCommandUpdated(UAVObject *)));
|
|
|
|
connect(widget, SIGNAL(sticksChanged(double, double, double, double)), this, SLOT(sticksChangedLocally(double, double, double, double)));
|
|
|
|
connect(this, SIGNAL(sticksChangedRemotely(double, double, double, double)), widget, SLOT(updateSticks(double, double, double, double)));
|
2011-02-02 00:01:52 +01:00
|
|
|
|
|
|
|
manualControlCommandUpdated(getManualControlCommand());
|
|
|
|
|
2012-07-09 18:18:57 +02:00
|
|
|
control_sock = new QUdpSocket(this);
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
connect(control_sock, SIGNAL(readyRead()), this, SLOT(readUDPCommand()));
|
2012-07-09 18:18:57 +02:00
|
|
|
|
2011-02-02 00:01:52 +01:00
|
|
|
joystickTime.start();
|
2013-05-19 16:37:30 +02:00
|
|
|
GCSControlPlugin *pl = dynamic_cast<GCSControlPlugin *>(plugin);
|
|
|
|
connect(pl->sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8)));
|
|
|
|
connect(pl->sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool)));
|
|
|
|
connect(pl->sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16)));
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
GCSControlGadget::~GCSControlGadget()
|
|
|
|
{
|
|
|
|
delete m_widget;
|
|
|
|
}
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration *config)
|
2011-02-02 00:01:52 +01:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration *>(config);
|
2011-02-02 00:01:52 +01:00
|
|
|
|
|
|
|
QList<int> ql = GCSControlConfig->getChannelsMapping();
|
2013-05-19 16:37:30 +02:00
|
|
|
rollChannel = ql.at(0);
|
|
|
|
pitchChannel = ql.at(1);
|
|
|
|
yawChannel = ql.at(2);
|
2011-02-02 00:01:52 +01:00
|
|
|
throttleChannel = ql.at(3);
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// if(control_sock->isOpen())
|
|
|
|
// control_sock->close();
|
|
|
|
control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(), QUdpSocket::ShareAddress);
|
2012-07-09 18:18:57 +02:00
|
|
|
|
2011-02-02 00:01:52 +01:00
|
|
|
controlsMode = GCSControlConfig->getControlsMode();
|
|
|
|
|
|
|
|
int i;
|
2013-05-19 16:37:30 +02:00
|
|
|
for (i = 0; i < 8; i++) {
|
|
|
|
buttonSettings[i].ActionID = GCSControlConfig->getbuttonSettings(i).ActionID;
|
|
|
|
buttonSettings[i].FunctionID = GCSControlConfig->getbuttonSettings(i).FunctionID;
|
|
|
|
buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount;
|
|
|
|
buttonSettings[i].Amount = GCSControlConfig->getbuttonSettings(i).Amount;
|
|
|
|
channelReverse[i] = GCSControlConfig->getChannelsReverse().at(i);
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
ManualControlCommand *GCSControlGadget::getManualControlCommand()
|
|
|
|
{
|
2011-02-02 00:01:52 +01:00
|
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
|
|
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
return dynamic_cast<ManualControlCommand *>(objManager->getObject(QString("ManualControlCommand")));
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj)
|
|
|
|
{
|
|
|
|
double roll = obj->getField("Roll")->getDouble();
|
|
|
|
double pitch = obj->getField("Pitch")->getDouble();
|
|
|
|
double yaw = obj->getField("Yaw")->getDouble();
|
2011-02-02 00:01:52 +01:00
|
|
|
double throttle = obj->getField("Throttle")->getDouble();
|
2013-04-20 17:48:23 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// necessary against having the wrong joystick profile chosen, which shows weird values
|
2013-05-02 00:22:41 +02:00
|
|
|
if (throttle > -1.0 && throttle <= 1.0) {
|
2013-04-20 19:31:35 +02:00
|
|
|
// convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1)
|
|
|
|
throttle = -1.0 + (throttle * 2.0);
|
2013-05-02 00:22:41 +02:00
|
|
|
} else {
|
|
|
|
// with the safety value (line 206), this helps keep the sticks insde the margins
|
|
|
|
if (throttle <= -1.0) {
|
|
|
|
throttle = -1.0;
|
|
|
|
} else {
|
|
|
|
throttle = 1.0;
|
|
|
|
}
|
2013-04-20 19:31:35 +02:00
|
|
|
}
|
2013-04-20 17:48:23 +02:00
|
|
|
|
2011-02-02 00:01:52 +01:00
|
|
|
// Remap RPYT to left X/Y and right X/Y depending on mode
|
|
|
|
switch (controlsMode) {
|
|
|
|
case 1:
|
|
|
|
// Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
|
2013-05-19 16:37:30 +02:00
|
|
|
emit sticksChangedRemotely(yaw, -pitch, roll, throttle);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 2:
|
|
|
|
// Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
|
2013-05-19 16:37:30 +02:00
|
|
|
emit sticksChangedRemotely(yaw, throttle, roll, -pitch);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 3:
|
|
|
|
// Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
|
2013-05-19 16:37:30 +02:00
|
|
|
emit sticksChangedRemotely(roll, -pitch, yaw, throttle);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 4:
|
|
|
|
// Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
|
2013-05-19 16:37:30 +02:00
|
|
|
emit sticksChangedRemotely(roll, throttle, yaw, -pitch);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
2013-05-19 16:37:30 +02:00
|
|
|
Update the manual commands - maps depending on mode
|
|
|
|
*/
|
|
|
|
void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY)
|
|
|
|
{
|
|
|
|
ManualControlCommand *obj = getManualControlCommand();
|
|
|
|
double oldRoll = obj->getField("Roll")->getDouble();
|
|
|
|
double oldPitch = obj->getField("Pitch")->getDouble();
|
|
|
|
double oldYaw = obj->getField("Yaw")->getDouble();
|
2011-02-02 00:01:52 +01:00
|
|
|
double oldThrottle = obj->getField("Throttle")->getDouble();
|
|
|
|
|
|
|
|
double newRoll;
|
|
|
|
double newPitch;
|
|
|
|
double newYaw;
|
|
|
|
double newThrottle;
|
|
|
|
|
|
|
|
// Remap left X/Y and right X/Y to RPYT depending on mode
|
|
|
|
switch (controlsMode) {
|
|
|
|
case 1:
|
|
|
|
// Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
|
2013-05-19 16:37:30 +02:00
|
|
|
newRoll = rightX;
|
|
|
|
newPitch = -leftY;
|
|
|
|
newYaw = leftX;
|
2011-02-02 00:01:52 +01:00
|
|
|
newThrottle = rightY;
|
|
|
|
break;
|
|
|
|
case 2:
|
|
|
|
// Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
|
2013-05-19 16:37:30 +02:00
|
|
|
newRoll = rightX;
|
|
|
|
newPitch = -rightY;
|
|
|
|
newYaw = leftX;
|
2011-02-02 00:01:52 +01:00
|
|
|
newThrottle = leftY;
|
|
|
|
break;
|
|
|
|
case 3:
|
|
|
|
// Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
|
2013-05-19 16:37:30 +02:00
|
|
|
newRoll = leftX;
|
|
|
|
newPitch = -leftY;
|
|
|
|
newYaw = rightX;
|
2011-02-02 00:01:52 +01:00
|
|
|
newThrottle = rightY;
|
|
|
|
break;
|
|
|
|
case 4:
|
|
|
|
// Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
|
2013-05-19 16:37:30 +02:00
|
|
|
newRoll = leftX;
|
|
|
|
newPitch = -rightY;
|
|
|
|
newYaw = rightX;
|
2011-02-02 00:01:52 +01:00
|
|
|
newThrottle = leftY;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// check if buttons have control over this axis... if so don't update it
|
|
|
|
int buttonRollControl = 0;
|
|
|
|
int buttonPitchControl = 0;
|
|
|
|
int buttonYawControl = 0;
|
|
|
|
int buttonThrottleControl = 0;
|
|
|
|
for (int i = 0; i < 8; i++) {
|
|
|
|
if ((buttonSettings[i].FunctionID == 1) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
|
|
|
|
buttonRollControl = 1;
|
|
|
|
}
|
|
|
|
if ((buttonSettings[i].FunctionID == 2) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
|
|
|
|
buttonPitchControl = 1;
|
|
|
|
}
|
|
|
|
if ((buttonSettings[i].FunctionID == 3) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
|
|
|
|
buttonYawControl = 1;
|
|
|
|
}
|
|
|
|
if ((buttonSettings[i].FunctionID == 4) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
|
|
|
|
buttonThrottleControl = 1;
|
|
|
|
}
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// if we are not in local gcs control mode, ignore the joystick input
|
|
|
|
if (((GCSControlGadgetWidget *)m_widget)->getGCSControl() == false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) {
|
2012-07-09 18:18:57 +02:00
|
|
|
return;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
// if (newThrottle != oldThrottle) {
|
|
|
|
// convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1)
|
|
|
|
newThrottle = (newThrottle + 1.0) / 2.0;
|
|
|
|
|
|
|
|
// safety value to stop the motors from spinning at 0% throttle
|
|
|
|
if (newThrottle <= 0.01) {
|
|
|
|
newThrottle = -1;
|
|
|
|
}
|
|
|
|
// }
|
|
|
|
|
2011-02-02 00:01:52 +01:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
if ((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
|
|
|
|
if (buttonRollControl == 0) {
|
|
|
|
obj->getField("Roll")->setDouble(newRoll);
|
|
|
|
}
|
|
|
|
if (buttonPitchControl == 0) {
|
|
|
|
obj->getField("Pitch")->setDouble(newPitch);
|
|
|
|
}
|
|
|
|
if (buttonYawControl == 0) {
|
|
|
|
obj->getField("Yaw")->setDouble(newYaw);
|
|
|
|
}
|
|
|
|
if (buttonThrottleControl == 0) {
|
|
|
|
obj->getField("Throttle")->setDouble(newThrottle);
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Thrust")->setDouble(newThrottle);
|
2013-05-02 00:22:41 +02:00
|
|
|
}
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Connected")->setValue("True");
|
2011-02-02 00:01:52 +01:00
|
|
|
obj->updated();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void GCSControlGadget::gamepads(quint8 count)
|
|
|
|
{
|
2012-08-23 18:03:17 +02:00
|
|
|
Q_UNUSED(count);
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// sdlGamepad.setGamepad(0);
|
|
|
|
// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE);
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
2012-07-09 18:18:57 +02:00
|
|
|
void GCSControlGadget::readUDPCommand()
|
|
|
|
{
|
|
|
|
double pitch, yaw, roll, throttle;
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2012-07-09 18:18:57 +02:00
|
|
|
while (control_sock->hasPendingDatagrams()) {
|
|
|
|
QByteArray datagram;
|
|
|
|
datagram.resize(control_sock->pendingDatagramSize());
|
|
|
|
control_sock->readDatagram(datagram.data(), datagram.size());
|
|
|
|
QDataStream readData(datagram);
|
|
|
|
bool badPack = false;
|
2013-05-19 16:37:30 +02:00
|
|
|
int state = 0;
|
|
|
|
while (!readData.atEnd() && !badPack) {
|
2012-07-09 18:18:57 +02:00
|
|
|
double buffer;
|
|
|
|
readData >> buffer;
|
2013-05-19 16:37:30 +02:00
|
|
|
switch (state) {
|
2012-07-09 18:18:57 +02:00
|
|
|
case 0:
|
2013-05-19 16:37:30 +02:00
|
|
|
if (buffer == 42) {
|
2012-07-09 18:18:57 +02:00
|
|
|
state = 1;
|
2013-05-19 16:37:30 +02:00
|
|
|
} else {
|
|
|
|
state = 0;
|
2012-07-09 18:18:57 +02:00
|
|
|
badPack = true;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case 1:
|
2013-05-19 16:37:30 +02:00
|
|
|
pitch = buffer;
|
|
|
|
state = 2;
|
2012-07-09 18:18:57 +02:00
|
|
|
break;
|
|
|
|
case 2:
|
2013-05-19 16:37:30 +02:00
|
|
|
yaw = buffer;
|
|
|
|
state = 3;
|
2012-07-09 18:18:57 +02:00
|
|
|
break;
|
|
|
|
case 3:
|
2013-05-19 16:37:30 +02:00
|
|
|
roll = buffer;
|
|
|
|
state = 4;
|
2012-07-09 18:18:57 +02:00
|
|
|
break;
|
|
|
|
case 4:
|
|
|
|
throttle = buffer;
|
2013-05-19 16:37:30 +02:00
|
|
|
state = 5;
|
2012-07-09 18:18:57 +02:00
|
|
|
break;
|
|
|
|
case 5:
|
2013-05-19 16:37:30 +02:00
|
|
|
if (buffer != 36 || !readData.atEnd()) {
|
|
|
|
badPack = true;
|
|
|
|
}
|
2012-07-09 18:18:57 +02:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
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()) {
|
2012-07-09 18:18:57 +02:00
|
|
|
obj->getField("Throttle")->setDouble(constrain(throttle));
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Thrust")->setDouble(constrain(throttle));
|
2012-07-09 18:18:57 +02:00
|
|
|
update = true;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
if (update) {
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Connected")->setValue("True");
|
2013-05-19 16:37:30 +02:00
|
|
|
obj->updated();
|
|
|
|
}
|
2012-07-09 18:18:57 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle;
|
|
|
|
}
|
|
|
|
|
|
|
|
double GCSControlGadget::constrain(double value)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
if (value < -1) {
|
2012-07-09 18:18:57 +02:00
|
|
|
return -1;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
if (value > 1) {
|
2012-07-09 18:18:57 +02:00
|
|
|
return 1;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2012-07-09 18:18:57 +02:00
|
|
|
return value;
|
|
|
|
}
|
|
|
|
|
2011-02-02 00:01:52 +01:00
|
|
|
void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0) && (pressed)) { // this button is configured
|
2011-02-02 00:01:52 +01:00
|
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
|
|
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
|
2013-05-19 16:37:30 +02:00
|
|
|
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
|
2011-02-02 00:01:52 +01:00
|
|
|
bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
|
2012-07-09 18:18:57 +02:00
|
|
|
bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
|
2011-02-02 00:01:52 +01:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
switch (buttonSettings[number].ActionID) {
|
|
|
|
case 1: // increase
|
|
|
|
if (currentCGSControl) {
|
|
|
|
switch (buttonSettings[number].FunctionID) {
|
|
|
|
case 1: // Roll
|
|
|
|
obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount));
|
|
|
|
break;
|
|
|
|
case 2: // Pitch
|
|
|
|
obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 3: // Yaw
|
|
|
|
obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 4: // Throttle
|
|
|
|
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() + buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 2: // decrease
|
|
|
|
if (currentCGSControl) {
|
|
|
|
switch (buttonSettings[number].FunctionID) {
|
|
|
|
case 1: // Roll
|
|
|
|
obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 2: // Pitch
|
|
|
|
obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 3: // Yaw
|
|
|
|
obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount));
|
|
|
|
break;
|
|
|
|
case 4: // Throttle
|
|
|
|
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() - buttonSettings[number].Amount));
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 3: // toggle
|
|
|
|
switch (buttonSettings[number].FunctionID) {
|
|
|
|
case 1: // Armed
|
|
|
|
if (currentCGSControl) {
|
2014-03-02 22:20:18 +01:00
|
|
|
if (((GCSControlGadgetWidget *)m_widget)->getArmed()) {
|
|
|
|
((GCSControlGadgetWidget *)m_widget)->setArmed(false);
|
2013-05-19 16:37:30 +02:00
|
|
|
} else {
|
2014-03-02 22:20:18 +01:00
|
|
|
((GCSControlGadgetWidget *)m_widget)->setArmed(true);
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
case 2: // GCS Control
|
|
|
|
// Toggle the GCS Control checkbox, its built in signalling will handle the update to OP
|
2011-02-02 00:01:52 +01:00
|
|
|
((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl);
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
|
|
|
case 3: // UDP Control
|
|
|
|
if (currentCGSControl) {
|
|
|
|
((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl);
|
|
|
|
}
|
2012-07-09 18:18:57 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2014-03-02 22:20:18 +01:00
|
|
|
obj->getField("Connected")->setValue("True");
|
2011-02-02 00:01:52 +01:00
|
|
|
obj->updated();
|
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
// buttonSettings[number].ActionID NIDT
|
|
|
|
// buttonSettings[number].FunctionID -RPYTAC
|
|
|
|
// buttonSettings[number].Amount
|
2011-02-02 00:01:52 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
void GCSControlGadget::axesValues(QListInt16 values)
|
|
|
|
{
|
|
|
|
int chMax = values.length();
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2011-06-19 00:55:11 +02:00
|
|
|
if (rollChannel >= chMax || pitchChannel >= chMax ||
|
2013-05-19 16:37:30 +02:00
|
|
|
yawChannel >= chMax || throttleChannel >= chMax) {
|
2011-02-02 00:01:52 +01:00
|
|
|
qDebug() << "GCSControl: configuration is inconsistent with current joystick! Aborting update.";
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
double rValue = (rollChannel > -1) ? values[rollChannel] : 0;
|
|
|
|
double pValue = (pitchChannel > -1) ? values[pitchChannel] : 0;
|
|
|
|
double yValue = (yawChannel > -1) ? values[yawChannel] : 0;
|
|
|
|
double tValue = (throttleChannel > -1) ? values[throttleChannel] : 0;
|
2013-05-19 16:37:30 +02:00
|
|
|
double max = 32767;
|
2011-02-02 00:01:52 +01:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
if (rollChannel > -1) {
|
|
|
|
if (channelReverse[rollChannel] == true) {
|
|
|
|
rValue = -rValue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (pitchChannel > -1) {
|
|
|
|
if (channelReverse[pitchChannel] == true) {
|
|
|
|
pValue = -pValue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (yawChannel > -1) {
|
|
|
|
if (channelReverse[yawChannel] == true) {
|
|
|
|
yValue = -yValue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (throttleChannel > -1) {
|
|
|
|
if (channelReverse[throttleChannel] == true) {
|
|
|
|
tValue = -tValue;
|
|
|
|
}
|
|
|
|
}
|
2011-02-02 00:01:52 +01:00
|
|
|
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
if (joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) {
|
2011-02-02 00:01:52 +01:00
|
|
|
joystickTime.restart();
|
|
|
|
// Remap RPYT to left X/Y and right X/Y depending on mode
|
|
|
|
// Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
|
|
|
|
// Mode 2: LeftX = Yaw, LeftY = THrottle, RightX = Roll, RightY = Pitch
|
|
|
|
// Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
|
|
|
|
// Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
|
|
|
|
switch (controlsMode) {
|
|
|
|
case 1:
|
2013-05-19 16:37:30 +02:00
|
|
|
sticksChangedLocally(yValue / max, -pValue / max, rValue / max, -tValue / max);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 2:
|
2013-05-19 16:37:30 +02:00
|
|
|
sticksChangedLocally(yValue / max, -tValue / max, rValue / max, -pValue / max);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 3:
|
2013-05-19 16:37:30 +02:00
|
|
|
sticksChangedLocally(rValue / max, -pValue / max, yValue / max, -tValue / max);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
case 4:
|
2013-05-19 16:37:30 +02:00
|
|
|
sticksChangedLocally(rValue / max, -tValue / max, yValue / max, -pValue / max);
|
2011-02-02 00:01:52 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
double GCSControlGadget::bound(double input)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
if (input > 1.0) {
|
|
|
|
return 1.0;
|
|
|
|
}
|
|
|
|
if (input < -1.0) {
|
|
|
|
return -1.0;
|
|
|
|
}
|
2011-02-02 00:01:52 +01:00
|
|
|
return input;
|
|
|
|
}
|
|
|
|
|
|
|
|
double GCSControlGadget::wrap(double input)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
while (input > 1.0) {
|
|
|
|
input -= 2.0;
|
|
|
|
}
|
|
|
|
while (input < -1.0) {
|
|
|
|
input += 2.0;
|
|
|
|
}
|
2011-02-02 00:01:52 +01:00
|
|
|
return input;
|
|
|
|
}
|