QGCVehicleConfig.cc

// On Windows (for VS2010) stdint.h contains the limits normally contained in limits.h
// It also needs the __STDC_LIMIT_MACROS macro defined in order to include them (done
// in qgroundcontrol.pri).
#ifdef WIN32
#include <stdint.h>
#else
#include <limits.h>
#endif

#include <QTimer>
#include <QDir>
#include <QXmlStreamReader>
#include <QMessageBox>

#include "QGCVehicleConfig.h"
#include "UASManager.h"
#include "QGC.h"
#include "QGCToolWidget.h"
#include "ui_QGCVehicleConfig.h"

QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
    QWidget(parent),
    mav(NULL),
    chanCount(0),
    rcRoll(0.0f),
    rcPitch(0.0f),
    rcYaw(0.0f),
    rcThrottle(0.0f),
    rcMode(0.0f),
    rcAux1(0.0f),
    rcAux2(0.0f),
    rcAux3(0.0f),
    changed(true),
    rc_mode(RC_MODE_NONE),
    calibrationEnabled(false),
    ui(new Ui::QGCVehicleConfig)
{
    doneLoadingConfig = false;
    systemTypeToParamMap["FIXED_WING"] = new QMap<QString,QGCToolWidget*>();
    systemTypeToParamMap["QUADROTOR"] = new QMap<QString,QGCToolWidget*>();
    systemTypeToParamMap["GROUND_ROVER"] = new QMap<QString,QGCToolWidget*>();
    systemTypeToParamMap["BOAT"] = new QMap<QString,QGCToolWidget*>();
    libParamToWidgetMap = new QMap<QString,QGCToolWidget*>();

    setObjectName("QGC_VEHICLECONFIG");
    ui->setupUi(this);

    ui->rollWidget->setOrientation(Qt::Horizontal);
    ui->rollWidget->setName("Roll");
    ui->yawWidget->setOrientation(Qt::Horizontal);
    ui->yawWidget->setName("Yaw");
    ui->pitchWidget->setName("Pitch");
    ui->throttleWidget->setName("Throttle");
    ui->radio5Widget->setOrientation(Qt::Horizontal);
    ui->radio5Widget->setName("Radio 5");
    ui->radio6Widget->setOrientation(Qt::Horizontal);
    ui->radio6Widget->setName("Radio 6");
    ui->radio7Widget->setOrientation(Qt::Horizontal);
    ui->radio7Widget->setName("Radio 7");
    ui->radio8Widget->setOrientation(Qt::Horizontal);
    ui->radio8Widget->setName("Radio 8");

    connect(ui->rcMenuButton,SIGNAL(clicked()),this,SLOT(rcMenuButtonClicked()));
    connect(ui->sensorMenuButton,SIGNAL(clicked()),this,SLOT(sensorMenuButtonClicked()));
    connect(ui->generalMenuButton,SIGNAL(clicked()),this,SLOT(generalMenuButtonClicked()));
    connect(ui->advancedMenuButton,SIGNAL(clicked()),this,SLOT(advancedMenuButtonClicked()));


    requestCalibrationRC();
    if (mav) mav->requestParameter(0, "RC_TYPE");

    ui->rcModeComboBox->setCurrentIndex((int)rc_mode - 1);

    ui->rcCalibrationButton->setCheckable(true);
    connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool)));
    connect(ui->setButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
    connect(ui->refreshButton,SIGNAL(clicked()),mav,SLOT(requestParameters()));
    connect(ui->rcModeComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setRCModeIndex(int)));
    //connect(ui->setTrimButton, SIGNAL(clicked()), this, SLOT(setTrimPositions()));

    /* Connect RC mapping assignments */
    connect(ui->rollSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setRollChan(int)));
    connect(ui->pitchSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setPitchChan(int)));
    connect(ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYawChan(int)));
    connect(ui->throttleSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setThrottleChan(int)));
    connect(ui->modeSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setModeChan(int)));
    connect(ui->aux1SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux1Chan(int)));
    connect(ui->aux2SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux2Chan(int)));
    connect(ui->aux3SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux3Chan(int)));

    // Connect RC reverse assignments
    connect(ui->invertCheckBox, SIGNAL(clicked(bool)), this, SLOT(setRollInverted(bool)));
    connect(ui->invertCheckBox_2, SIGNAL(clicked(bool)), this, SLOT(setPitchInverted(bool)));
    connect(ui->invertCheckBox_3, SIGNAL(clicked(bool)), this, SLOT(setYawInverted(bool)));
    connect(ui->invertCheckBox_4, SIGNAL(clicked(bool)), this, SLOT(setThrottleInverted(bool)));
    connect(ui->invertCheckBox_5, SIGNAL(clicked(bool)), this, SLOT(setModeInverted(bool)));
    connect(ui->invertCheckBox_6, SIGNAL(clicked(bool)), this, SLOT(setAux1Inverted(bool)));
    connect(ui->invertCheckBox_7, SIGNAL(clicked(bool)), this, SLOT(setAux2Inverted(bool)));
    connect(ui->invertCheckBox_8, SIGNAL(clicked(bool)), this, SLOT(setAux3Inverted(bool)));

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));

    setActiveUAS(UASManager::instance()->getActiveUAS());

    for (unsigned int i = 0; i < chanMax; i++)
    {
        rcValue[i] = UINT16_MAX;
        rcMapping[i] = i;
    }

    updateTimer.setInterval(150);
    connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateView()));
    updateTimer.start();

    ui->advancedGroupBox->hide();
    connect(ui->advancedCheckBox,SIGNAL(toggled(bool)),ui->advancedGroupBox,SLOT(setShown(bool)));
}
void QGCVehicleConfig::rcMenuButtonClicked()
{
    ui->stackedWidget->setCurrentIndex(0);
}

void QGCVehicleConfig::sensorMenuButtonClicked()
{
    ui->stackedWidget->setCurrentIndex(1);
}

void QGCVehicleConfig::generalMenuButtonClicked()
{
    ui->stackedWidget->setCurrentIndex(ui->stackedWidget->count()-2);
}

void QGCVehicleConfig::advancedMenuButtonClicked()
{
    ui->stackedWidget->setCurrentIndex(ui->stackedWidget->count()-1);
}

QGCVehicleConfig::~QGCVehicleConfig()
{
    delete ui;
}

void QGCVehicleConfig::setRCModeIndex(int newRcMode)
{
    if (newRcMode > 0 && newRcMode < 5)
    {
        rc_mode = (enum RC_MODE) (newRcMode+1);
        changed = true;
    }
}

void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
{
    if (enabled)
    {
        startCalibrationRC();
    }
    else
    {
        stopCalibrationRC();
    }
}

void QGCVehicleConfig::setTrimPositions()
{
    // Set trim to min if stick is close to min
    if (abs(rcValue[rcMapping[3]] - rcMin[rcMapping[3]]) < 100)
    {
        rcTrim[rcMapping[3]] = rcMin[rcMapping[3]];   // throttle
    }
    // Set trim to max if stick is close to max
    else if (abs(rcValue[rcMapping[3]] - rcMax[rcMapping[3]]) < 100)
    {
        rcTrim[rcMapping[3]] = rcMax[rcMapping[3]];   // throttle
    }
    else
    {

        // Reject
        QMessageBox msgBox;
        msgBox.setText(tr("Throttle Stick Trim Position Invalid"));
        msgBox.setInformativeText(tr("The throttle stick is not in the min position. Please set it to the minimum value"));
        msgBox.setStandardButtons(QMessageBox::Ok);
        msgBox.setDefaultButton(QMessageBox::Ok);
        (void)msgBox.exec();
    }

    // Set trim for roll, pitch, yaw, throttle
    rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll
    rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch
    rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw

    rcTrim[rcMapping[4]] = ((rcMax[rcMapping[4]] - rcMin[rcMapping[4]]) / 2.0f) + rcMin[rcMapping[4]];   // mode sw
    rcTrim[rcMapping[5]] = ((rcMax[rcMapping[5]] - rcMin[rcMapping[5]]) / 2.0f) + rcMin[rcMapping[5]];   // aux 1
    rcTrim[rcMapping[6]] = ((rcMax[rcMapping[6]] - rcMin[rcMapping[6]]) / 2.0f) + rcMin[rcMapping[6]];   // aux 2
    rcTrim[rcMapping[7]] = ((rcMax[rcMapping[7]] - rcMin[rcMapping[7]]) / 2.0f) + rcMin[rcMapping[7]];   // aux 3
}

void QGCVehicleConfig::detectChannelInversion()
{