gcscontrolgadget.cpp

Go to the documentation of this file.

/*
 * 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, see <http://www.gnu.org/licenses/>
 */
#include "gcscontrol.h"
#include "gcscontrolgadget.h"
#include "gcscontrolgadgetwidget.h"
#include "gcscontrolgadgetconfiguration.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjects/uavobjectmanager.h"
#include "uavobjects/uavobject.h"
#include <QDebug>

#define JOYSTICK_UPDATE_RATE 50

GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widget, QWidget *parent,
                                   QObject *plugin)
    : IUAVGadget(classId, parent)
    , m_widget(widget)
    , controlsMode(0)
{
    connect(getManualControlCommand(), &UAVObject::objectUpdated, this,
            &GCSControlGadget::manualControlCommandUpdated);
    connect(widget, &GCSControlGadgetWidget::sticksChanged, this,
            &GCSControlGadget::sticksChangedLocally);
    connect(widget, &GCSControlGadgetWidget::controlEnabled, this,
            &GCSControlGadget::enableControl);
    connect(this, &GCSControlGadget::sticksChangedRemotely, widget,
            &GCSControlGadgetWidget::updateSticks);
    connect(widget, &GCSControlGadgetWidget::flightModeChangedLocally, this,
            &GCSControlGadget::flightModeChanged);

    manualControlCommandUpdated(getManualControlCommand());

    joystickTime.start();

#if defined(USE_SDL)
    GCSControl *pl = dynamic_cast<GCSControl *>(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)));
#else
    Q_UNUSED(plugin)
#endif
}

GCSControlGadget::~GCSControlGadget()
{
    delete m_widget;
}

void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration *config)
{
    GCSControlGadgetConfiguration *GCSControlConfig =
        qobject_cast<GCSControlGadgetConfiguration *>(config);

    QList<int> ql = GCSControlConfig->getChannelsMapping();
    rollChannel = ql.at(0);
    pitchChannel = ql.at(1);
    yawChannel = ql.at(2);
    throttleChannel = ql.at(3);

    controlsMode = GCSControlConfig->getControlsMode();
    gcsReceiverMode = GCSControlConfig->getGcsReceiverMode();
    m_widget->allowGcsControl(gcsReceiverMode);

    for (unsigned int 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);
    }
}

void GCSControlGadget::enableControl(bool enable)
{
    enableSending = enable;
    if (enableSending)
        getGcsControl()->beginGCSControl();
    else
        getGcsControl()->endGCSControl();
}

ManualControlCommand *GCSControlGadget::getManualControlCommand()
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    return dynamic_cast<ManualControlCommand *>(
        objManager->getObject(QString("ManualControlCommand")));
}

GCSControl *GCSControlGadget::getGcsControl()
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    GCSControl *gcsControl = pm->getObject<GCSControl>();
    Q_ASSERT(gcsControl);
    return gcsControl;
}

void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj)
{

    // Show updates from transmitter when we're not sending.
    if (enableSending)
        return;

    double roll = obj->getField("Roll")->getDouble();
    double pitch = obj->getField("Pitch")->getDouble();
    double yaw = obj->getField("Yaw")->getDouble();
    double throttle = obj->getField("Throttle")->getDouble();
    // 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
        emit sticksChangedRemotely(yaw, -pitch, roll, throttle);
        break;
    case 2:
        // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
        emit sticksChangedRemotely(yaw, throttle, roll, -pitch);
        break;
    case 3:
        // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
        emit sticksChangedRemotely(roll, -pitch, yaw, throttle);
        break;
    case 4:
        // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
        emit sticksChangedRemotely(roll, throttle, yaw, -pitch);
        break;
    }
}

void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX,
                                            double rightY, double arming)
{
    if (enableSending)
        setGcsReceiver(leftX, leftY, rightX, rightY, arming);
}

void GCSControlGadget::setGcsReceiver(double leftX, double leftY, double rightX, double rightY,
                                      double arming)
{
    GCSControl *ctr = getGcsControl();
    Q_ASSERT(ctr);
    if (ctr == NULL)
        return;

    double newRoll = 0;
    double newPitch = 0;
    double newYaw = 0;
    double newThrottle = 0;

    // 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
        newRoll = rightX;
        newPitch = -leftY;
        newYaw = leftX;
        newThrottle = rightY;
        break;
    case 2:
        // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
        newRoll = rightX;
        newPitch = -rightY;
        newYaw = leftX;
        newThrottle = leftY;
        break;
    case 3:
        // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
        newRoll = leftX;
        newPitch = -leftY;
        newYaw = rightX;
        newThrottle = rightY;
        break;
    case 4:
        // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
        newRoll = leftX;
        newPitch = -rightY;
        newYaw = rightX;
        newThrottle = leftY;
        break;
    }

    ctr->setThrottle(newThrottle);
    ctr->setRoll(newRoll);
    ctr->setPitch(newPitch);
    ctr->setYaw(newYaw);
    ctr->setArming(arming);

    switch (controlsMode) {
    case 1:
        // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle
        emit sticksChangedRemotely(newYaw, -newPitch, newRoll, newThrottle);
        break;
    case 2:
        // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch
        emit sticksChangedRemotely(newYaw, newThrottle, newRoll, -newPitch);
        break;
    case 3:
        // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle
        emit sticksChangedRemotely(newRoll, -newPitch, newYaw, newThrottle);
        break;
    case 4:
        // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch;
        emit sticksChangedRemotely(newRoll, newThrottle, newYaw, -newPitch);
        break;
    }
}

void GCSControlGadget::flightModeChanged(ManualControlSettings::FlightModePositionOptions mode)
{
    if (enableSending)
        getGcsControl()->setFlightMode(mode);
}

void GCSControlGadget::gamepads(quint8 count)
{
    Q_UNUSED(count);
}

double GCSControlGadget::constrain(double value)
{
    if (value < -1)
        return -1;
    if (value > 1)
        return 1;
    return value;
}

#if defined(USE_SDL)
void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
{
    if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0)
        && (pressed)) { // this button is configured
        ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
        UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
        UAVDataObject *obj =
            dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
        bool currentCGSControl =
            (dynamic_cast<GCSControlGadgetWidget *>(m_widget))->getGCSControl();

        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));
                    break;
                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));
                    break;
                }
            }
            break;
        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));
                    break;
                case 2: // Pitch
                    obj->getField("Pitch")->setValue(
                        bound(obj->getField("Pitch")->getValue().toDouble()
                              - buttonSettings[number].Amount));
                    break;
                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));
                    break;
                }
            }
            break;
        case 3: // toggle
            switch (buttonSettings[number].FunctionID) {
            case 1: // Armed
                if (currentCGSControl) {
                    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
                    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
                    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(
                        objManager->getObject(QString("FlightStatus")));

                    if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) {
                        obj->getField("Armed")->setValue("Disarmed");
                    } else {
                        obj->getField("Armed")->setValue("Armed");
                    }
                }
                break;
            case 2: // GCS Control
                // Toggle the GCS Control checkbox, its built in signalling will handle the update
                // to OP
                (dynamic_cast<GCSControlGadgetWidget *>(m_widget))
                    ->setGCSControl(!currentCGSControl);

                break;
            }

            break;
        }

        obj->updated();
    }
    // buttonSettings[number].ActionID NIDT
    // buttonSettings[number].FunctionID -RPYTAC
    // buttonSettings[number].Amount
}

void GCSControlGadget::axesValues(QListInt16 values)
{
    int chMax = values.length();
    if (rollChannel >= chMax || pitchChannel >= chMax || yawChannel >= chMax
        || throttleChannel >= chMax) {
        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;
    double max = 32767;

    /* TODO: need to appropriately plumb */
    double arming = -1;

    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;

    if (joystickTime.elapsed() > JOYSTICK_UPDATE_RATE) {
        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:
            sticksChangedLocally(yValue / max, -pValue / max, rValue / max, -tValue / max, arming);
            break;
        case 2:
            sticksChangedLocally(yValue / max, -tValue / max, rValue / max, -pValue / max, arming);
            break;
        case 3:
            sticksChangedLocally(rValue / max, -pValue / max, yValue / max, -tValue / max, arming);
            break;
        case 4:
            sticksChangedLocally(rValue / max, -tValue / max, yValue / max, -pValue / max, arming);
            break;
        }
    }
}
#endif

double GCSControlGadget::bound(double input)
{
    if (input > 1.0)
        return 1.0;
    if (input < -1.0)
        return -1.0;
    return input;
}

double GCSControlGadget::wrap(double input)
{
    while (input > 1.0)
        input -= 2.0;
    while (input < -1.0)
        input += 2.0;
    return input;
}