ground/gcs/src/plugins/gcscontrol/gcscontrolgadget.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       GCSControlgadget.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin
   9  * @{
  10  * @brief A gadget to control the UAV, either from the keyboard or a joystick
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27 #include "gcscontrolgadget.h"
  28 #include "gcscontrolgadgetwidget.h"
  29 #include "gcscontrolgadgetconfiguration.h"
  30 #include "extensionsystem/pluginmanager.h"
  31 #include "uavobjectmanager.h"
  32 #include "uavobject.h"
  33 #include <QDebug>
  34
  35 #define JOYSTICK_UPDATE_RATE 50
  36
  37 GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent, QObject *plugin) :
  38     IUAVGadget(classId, parent),
  39     m_widget(widget)
  40 {
  41     connect(getManualControlCommand(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(manualControlCommandUpdated(UAVObject *)));
  42     connect(widget, SIGNAL(sticksChanged(double, double, double, double)), this, SLOT(sticksChangedLocally(double, double, double, double)));
  43     connect(this, SIGNAL(sticksChangedRemotely(double, double, double, double)), widget, SLOT(updateSticks(double, double, double, double)));
  44
  45     manualControlCommandUpdated(getManualControlCommand());
  46
  47     control_sock = new QUdpSocket(this);
  48
  49     connect(control_sock, SIGNAL(readyRead()), this, SLOT(readUDPCommand()));
  50
  51     joystickTime.start();
  52     GCSControlPlugin *pl = dynamic_cast<GCSControlPlugin *>(plugin);
  53     connect(pl->sdlGamepad, SIGNAL(gamepads(quint8)), this, SLOT(gamepads(quint8)));
  54     connect(pl->sdlGamepad, SIGNAL(buttonState(ButtonNumber, bool)), this, SLOT(buttonState(ButtonNumber, bool)));
  55     connect(pl->sdlGamepad, SIGNAL(axesValues(QListInt16)), this, SLOT(axesValues(QListInt16)));
  56 }
  57
  58 GCSControlGadget::~GCSControlGadget()
  59 {
  60     delete m_widget;
  61 }
  62
  63 void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration *config)
  64 {
  65     GCSControlGadgetConfiguration *GCSControlConfig = qobject_cast< GCSControlGadgetConfiguration *>(config);
  66
  67     QList<int> ql = GCSControlConfig->getChannelsMapping();
  68     rollChannel     = ql.at(0);
  69     pitchChannel    = ql.at(1);
  70     yawChannel      = ql.at(2);
  71     throttleChannel = ql.at(3);
  72
  73     // if(control_sock->isOpen())
  74     // control_sock->close();
  75     control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(), QUdpSocket::ShareAddress);
  76
  77     controlsMode = GCSControlConfig->getControlsMode();
  78
  79     int i;
  80     for (i = 0; i < 8; i++) {
  81         buttonSettings[i].ActionID   = GCSControlConfig->getbuttonSettings(i).ActionID;
  82         buttonSettings[i].FunctionID = GCSControlConfig->getbuttonSettings(i).FunctionID;
  83         buttonSettings[i].Amount     = GCSControlConfig->getbuttonSettings(i).Amount;
  84         buttonSettings[i].Amount     = GCSControlConfig->getbuttonSettings(i).Amount;
  85         channelReverse[i] = GCSControlConfig->getChannelsReverse().at(i);
  86     }
  87 }
  88
  89 ManualControlCommand *GCSControlGadget::getManualControlCommand()
  90 {
  91     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  92     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
  93
  94     return dynamic_cast<ManualControlCommand *>(objManager->getObject(QString("ManualControlCommand")));
  95 }
  96
  97 void GCSControlGadget::manualControlCommandUpdated(UAVObject *manualControlCommand)
  98 {
  99     double roll     = manualControlCommand->getField("Roll")->getDouble();
 100     double pitch    = manualControlCommand->getField("Pitch")->getDouble();
 101     double yaw      = manualControlCommand->getField("Yaw")->getDouble();
 102     double throttle = manualControlCommand->getField("Throttle")->getDouble();
 103
 104     // necessary against having the wrong joystick profile chosen, which shows weird values
 105     if (throttle > -1.0 && throttle <= 1.0) {
 106         // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1)
 107         throttle = -1.0 + (throttle * 2.0);
 108     } else {
 109         // with the safety value (line 206), this helps keep the sticks insde the margins
 110         if (throttle <= -1.0) {
 111             throttle = -1.0;
 112         } else {
 113             throttle = 1.0;
 114         }
 115     }
 116
 117     // Remap RPYT to left X/Y and right X/Y depending on mode
 118     switch (controlsMode) {
 119     case 1:
 120         // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
 121         emit sticksChangedRemotely(yaw, -pitch, roll, throttle);
 122         break;
 123     case 2:
 124         // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
 125         emit sticksChangedRemotely(yaw, throttle, roll, -pitch);
 126         break;
 127     case 3:
 128         // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
 129         emit sticksChangedRemotely(roll, -pitch, yaw, throttle);
 130         break;
 131     case 4:
 132         // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
 133         emit sticksChangedRemotely(roll, throttle, yaw, -pitch);
 134         break;
 135     }
 136 }
 137
 138 /**
 139    Update the manual commands - maps depending on mode
 140  */
 141 void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY)
 142 {
 143     ManualControlCommand *manualControlCommand = getManualControlCommand();
 144     double oldRoll     = manualControlCommand->getField("Roll")->getDouble();
 145     double oldPitch    = manualControlCommand->getField("Pitch")->getDouble();
 146     double oldYaw      = manualControlCommand->getField("Yaw")->getDouble();
 147     double oldThrottle = manualControlCommand->getField("Throttle")->getDouble();
 148
 149     double newRoll     = 0.0;
 150     double newPitch    = 0.0;
 151     double newYaw      = 0.0;
 152     double newThrottle = 0.0;
 153
 154     // Remap left X/Y and right X/Y to RPYT depending on mode
 155     switch (controlsMode) {
 156     case 1:
 157         // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
 158         newRoll     = rightX;
 159         newPitch    = -leftY;
 160         newYaw      = leftX;
 161         newThrottle = rightY;
 162         break;
 163     case 2:
 164         // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
 165         newRoll     = rightX;
 166         newPitch    = -rightY;
 167         newYaw      = leftX;
 168         newThrottle = leftY;
 169         break;
 170     case 3:
 171         // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
 172         newRoll     = leftX;
 173         newPitch    = -leftY;
 174         newYaw      = rightX;
 175         newThrottle = rightY;
 176         break;
 177     case 4:
 178         // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
 179         newRoll     = leftX;
 180         newPitch    = -rightY;
 181         newYaw      = rightX;
 182         newThrottle = leftY;
 183         break;
 184     }
 185
 186     // check if buttons have control over this axis... if so don't update it
 187     int buttonRollControl     = 0;
 188     int buttonPitchControl    = 0;
 189     int buttonYawControl      = 0;
 190     int buttonThrottleControl = 0;
 191     for (int i = 0; i < 8; i++) {
 192         if ((buttonSettings[i].FunctionID == 1) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
 193             buttonRollControl = 1;
 194         }
 195         if ((buttonSettings[i].FunctionID == 2) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
 196             buttonPitchControl = 1;
 197         }
 198         if ((buttonSettings[i].FunctionID == 3) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
 199             buttonYawControl = 1;
 200         }
 201         if ((buttonSettings[i].FunctionID == 4) && ((buttonSettings[i].ActionID == 1) || (buttonSettings[i].ActionID == 2))) {
 202             buttonThrottleControl = 1;
 203         }
 204     }
 205
 206     // if we are not in local gcs control mode, ignore the joystick input
 207     if (((GCSControlGadgetWidget *)m_widget)->getGCSControl() == false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) {
 208         return;
 209     }
 210
 211     // if (newThrottle != oldThrottle) {
 212     // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1)
 213     newThrottle = (newThrottle + 1.0) / 2.0;
 214
 215     // safety value to stop the motors from spinning at 0% throttle
 216     if (newThrottle <= 0.01) {
 217         newThrottle = -1;
 218     }
 219     // }
 220
 221
 222     if ((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
 223         if (buttonRollControl == 0) {
 224             manualControlCommand->getField("Roll")->setDouble(newRoll);
 225         }
 226         if (buttonPitchControl == 0) {
 227             manualControlCommand->getField("Pitch")->setDouble(newPitch);
 228         }
 229         if (buttonYawControl == 0) {
 230             manualControlCommand->getField("Yaw")->setDouble(newYaw);
 231         }
 232         if (buttonThrottleControl == 0) {
 233             manualControlCommand->getField("Throttle")->setDouble(newThrottle);
 234             manualControlCommand->getField("Thrust")->setDouble(newThrottle);
 235         }
 236         manualControlCommand->getField("Connected")->setValue("True");
 237         manualControlCommand->updated();
 238     }
 239 }
 240
 241 void GCSControlGadget::gamepads(quint8 count)
 242 {
 243     Q_UNUSED(count);
 244
 245 // sdlGamepad.setGamepad(0);
 246 // sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE);
 247 }
 248
 249 void GCSControlGadget::readUDPCommand()
 250 {
 251     double pitch    = 0.0;
 252     double yaw      = 0.0;
 253     double roll     = 0.0;
 254     double throttle = 0.0;
 255
 256     while (control_sock->hasPendingDatagrams()) {
 257         QByteArray datagram;
 258         datagram.resize(control_sock->pendingDatagramSize());
 259         control_sock->readDatagram(datagram.data(), datagram.size());
 260         QDataStream readData(datagram);
 261         bool badPack = false;
 262         int state    = 0;
 263         while (!readData.atEnd() && !badPack) {
 264             double buffer;
 265             readData >> buffer;
 266             switch (state) {
 267             case 0:
 268                 if (buffer == 42) {
 269                     state = 1;
 270                 } else {
 271                     state   = 0;
 272                     badPack = true;
 273                 }
 274                 break;
 275             case 1:
 276                 pitch    = buffer;
 277                 state    = 2;
 278                 break;
 279             case 2:
 280                 yaw      = buffer;
 281                 state    = 3;
 282                 break;
 283             case 3:
 284                 roll     = buffer;
 285                 state    = 4;
 286                 break;
 287             case 4:
 288                 throttle = buffer;
 289                 state    = 5;
 290                 break;
 291             case 5:
 292                 if (buffer != 36 || !readData.atEnd()) {
 293                     badPack = true;
 294                 }
 295                 break;
 296             }
 297         }
 298         if (!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) {
 299             ManualControlCommand *manualControlCommand = getManualControlCommand();
 300             bool update = false;
 301
 302             if (pitch != manualControlCommand->getField("Pitch")->getDouble()) {
 303                 manualControlCommand->getField("Pitch")->setDouble(constrain(pitch));
 304                 update = true;
 305             }
 306             if (yaw != manualControlCommand->getField("Yaw")->getDouble()) {
 307                 manualControlCommand->getField("Yaw")->setDouble(constrain(yaw));
 308                 update = true;
 309             }
 310             if (roll != manualControlCommand->getField("Roll")->getDouble()) {
 311                 manualControlCommand->getField("Roll")->setDouble(constrain(roll));
 312                 update = true;
 313             }
 314             if (throttle != manualControlCommand->getField("Throttle")->getDouble()) {
 315                 manualControlCommand->getField("Throttle")->setDouble(constrain(throttle));
 316                 manualControlCommand->getField("Thrust")->setDouble(constrain(throttle));
 317                 update = true;
 318             }
 319             if (update) {
 320                 manualControlCommand->getField("Connected")->setValue("True");
 321                 manualControlCommand->updated();
 322             }
 323         }
 324     }
 325
 326     qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle;
 327 }
 328
 329 double GCSControlGadget::constrain(double value)
 330 {
 331     if (value < -1) {
 332         return -1;
 333     }
 334     if (value > 1) {
 335         return 1;
 336     }
 337     return value;
 338 }
 339
 340 void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
 341 {
 342     if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0) && (pressed)) { // this button is configured
 343         ExtensionSystem::PluginManager *pm  = ExtensionSystem::PluginManager::instance();
 344         UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
 345         UAVDataObject *manualControlCommand = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
 346         bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
 347         bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
 348
 349         switch (buttonSettings[number].ActionID) {
 350         case 1: // increase
 351             if (currentCGSControl) {
 352                 switch (buttonSettings[number].FunctionID) {
 353                 case 1: // Roll
 354                     manualControlCommand->getField("Roll")->setValue(bound(manualControlCommand->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount));
 355                     break;
 356                 case 2: // Pitch
 357                     manualControlCommand->getField("Pitch")->setValue(bound(manualControlCommand->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount));
 358                     break;
 359                 case 3: // Yaw
 360                     manualControlCommand->getField("Yaw")->setValue(wrap(manualControlCommand->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount));
 361                     break;
 362                 case 4: // Throttle
 363                     manualControlCommand->getField("Throttle")->setValue(bound(manualControlCommand->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
 364                     manualControlCommand->getField("Thrust")->setValue(bound(manualControlCommand->getField("Thrust")->getValue().toDouble() + buttonSettings[number].Amount));
 365                     break;
 366                 }
 367             }
 368             break;
 369         case 2: // decrease
 370             if (currentCGSControl) {
 371                 switch (buttonSettings[number].FunctionID) {
 372                 case 1: // Roll
 373                     manualControlCommand->getField("Roll")->setValue(bound(manualControlCommand->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount));
 374                     break;
 375                 case 2: // Pitch
 376                     manualControlCommand->getField("Pitch")->setValue(bound(manualControlCommand->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount));
 377                     break;
 378                 case 3: // Yaw
 379                     manualControlCommand->getField("Yaw")->setValue(wrap(manualControlCommand->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount));
 380                     break;
 381                 case 4: // Throttle
 382                     manualControlCommand->getField("Throttle")->setValue(bound(manualControlCommand->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
 383                     manualControlCommand->getField("Thrust")->setValue(bound(manualControlCommand->getField("Thrust")->getValue().toDouble() - buttonSettings[number].Amount));
 384                     break;
 385                 }
 386             }
 387             break;
 388         case 3: // toggle
 389             switch (buttonSettings[number].FunctionID) {
 390             case 1: // Armed
 391                 if (currentCGSControl) {
 392                     if (((GCSControlGadgetWidget *)m_widget)->getArmed()) {
 393                         ((GCSControlGadgetWidget *)m_widget)->setArmed(false);
 394                     } else {
 395                         ((GCSControlGadgetWidget *)m_widget)->setArmed(true);
 396                     }
 397                 }
 398                 break;
 399             case 2: // GCS Control
 400                     // Toggle the GCS Control checkbox, its built in signalling will handle the update to OP
 401                 ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl);
 402
 403                 break;
 404             case 3: // UDP Control
 405                 if (currentCGSControl) {
 406                     ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl);
 407                 }
 408
 409                 break;
 410             }
 411
 412             break;
 413         }
 414
 415         manualControlCommand->getField("Connected")->setValue("True");
 416         manualControlCommand->updated();
 417     }
 418     // buttonSettings[number].ActionID NIDT
 419     // buttonSettings[number].FunctionID -RPYTAC
 420     // buttonSettings[number].Amount
 421 }
 422
 423 void GCSControlGadget::axesValues(QListInt16 values)
 424 {
 425     int chMax = values.length();
 426
 427     if (rollChannel >= chMax || pitchChannel >= chMax ||
 428         yawChannel >= chMax || throttleChannel >= chMax) {
 429         qDebug() << "GCSControl: configuration is inconsistent with current joystick! Aborting update.";
 430         return;
 431     }
 432
 433     double rValue = (rollChannel > -1) ? values[rollChannel] : 0;
 434     double pValue = (pitchChannel > -1) ? values[pitchChannel] : 0;
 435     double yValue = (yawChannel > -1) ? values[yawChannel] : 0;
 436     double tValue = (throttleChannel > -1) ? values[throttleChannel] : 0;
 437     double max    = 32767;
 438
 439     if (rollChannel > -1) {
 440         if (channelReverse[rollChannel] == true) {
 441             rValue = -rValue;
 442         }
 443     }
 444     if (pitchChannel > -1) {
 445         if (channelReverse[pitchChannel] == true) {
 446             pValue = -pValue;
 447         }
 448     }
 449     if (yawChannel > -1) {
 450         if (channelReverse[yawChannel] == true) {
 451             yValue = -yValue;
 452         }
 453     }
 454     if (throttleChannel > -1) {
 455         if (channelReverse[throttleChannel] == true) {
 456             tValue = -tValue;
 457         }
 458     }
 459
 460
 461     if (joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) {
 462         joystickTime.restart();
 463         // Remap RPYT to left X/Y and right X/Y depending on mode
 464         // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
 465         // Mode 2: LeftX = Yaw, LeftY = THrottle, RightX = Roll, RightY = Pitch
 466         // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
 467         // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
 468         switch (controlsMode) {
 469         case 1:
 470             sticksChangedLocally(yValue / max, -pValue / max, rValue / max, -tValue / max);
 471             break;
 472         case 2:
 473             sticksChangedLocally(yValue / max, -tValue / max, rValue / max, -pValue / max);
 474             break;
 475         case 3:
 476             sticksChangedLocally(rValue / max, -pValue / max, yValue / max, -tValue / max);
 477             break;
 478         case 4:
 479             sticksChangedLocally(rValue / max, -tValue / max, yValue / max, -pValue / max);
 480             break;
 481         }
 482     }
 483 }
 484
 485
 486 double GCSControlGadget::bound(double input)
 487 {
 488     if (input > 1.0) {
 489         return 1.0;
 490     }
 491     if (input < -1.0) {
 492         return -1.0;
 493     }
 494     return input;
 495 }
 496
 497 double GCSControlGadget::wrap(double input)
 498 {
 499     while (input > 1.0) {
 500         input -= 2.0;
 501     }
 502     while (input < -1.0) {
 503         input += 2.0;
 504     }
 505     return input;
 506 }