QGCVehicleConfig.cc

    if (calibrationEnabled) {
        if (val < rcMin[chan])
        {
            rcMin[chan] = val;
        }

        if (val > rcMax[chan])
        {
            rcMax[chan] = val;
        }
    }

    // Raw value
    rcValue[chan] = val;

    // Normalized value
    float normalized;

    if (val >= rcTrim[chan]) {
        normalized = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
    }
    else {
        normalized = -(rcTrim[chan] - val)/(rcTrim[chan] - rcMin[chan]);
    }

    // Bound
    normalized = qBound(-1.0f, normalized, 1.0f);
    // Invert
    normalized = (rcRev[chan]) ? -1.0f*normalized : normalized;

    if (chan == rcMapping[0]) {
        // ROLL
        rcRoll = normalized;
    }
    if (chan == rcMapping[1]) {
        // PITCH
        rcPitch = normalized;
    }
    if (chan == rcMapping[2]) {
        rcYaw = normalized;
    }
    if (chan == rcMapping[3]) {
        // THROTTLE
        if (rcRev[chan]) {
            rcThrottle = 1.0f + normalized;
        } else {
            rcThrottle = normalized;
        }

        rcThrottle = qBound(0.0f, rcThrottle, 1.0f);
    }
    if (chan == rcMapping[4]) {
        // MODE SWITCH
        rcMode = normalized;
    }
    if (chan == rcMapping[5]) {
        // AUX1
        rcAux1 = normalized;
    }
    if (chan == rcMapping[6]) {
        // AUX2
        rcAux2 = normalized;
    }
    if (chan == rcMapping[7]) {
        // AUX3
        rcAux3 = normalized;
    }

    changed = true;

    //qDebug() << "RC CHAN:" << chan << "PPM:" << val << "NORMALIZED:" << normalized;
}

void QGCVehicleConfig::updateInvertedCheckboxes(int index)
{
    unsigned int mapindex = rcMapping[index];

    switch (mapindex)
    {
    case 0:
        ui->invertCheckBox->setChecked(rcRev[index]);
        break;
    case 1:
        ui->invertCheckBox_2->setChecked(rcRev[index]);
        break;
    case 2:
        ui->invertCheckBox_3->setChecked(rcRev[index]);
        break;
    case 3:
        ui->invertCheckBox_4->setChecked(rcRev[index]);
        break;
    case 4:
        ui->invertCheckBox_5->setChecked(rcRev[index]);
        break;
    case 5:
        ui->invertCheckBox_6->setChecked(rcRev[index]);
        break;
    case 6:
        ui->invertCheckBox_7->setChecked(rcRev[index]);
        break;
    case 7:
        ui->invertCheckBox_8->setChecked(rcRev[index]);
        break;
    }
}

void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
    Q_UNUSED(uas);
    Q_UNUSED(component);
    if (!doneLoadingConfig)
    {
        //We do not want to attempt to generate any UI elements until loading of the config file is complete.
        //We should re-request params later if needed, that is not implemented yet.
        return;
    }

    if (paramToWidgetMap.contains(parameterName))
    {
        //Main group of parameters of the selected airframe
        paramToWidgetMap.value(parameterName)->setParameterValue(uas,component,parameterName,value);
        if (toolToBoxMap.contains(paramToWidgetMap.value(parameterName)))
        {
            toolToBoxMap[paramToWidgetMap.value(parameterName)]->show();
        }
        else
        {
            qCritical() << "Widget with no box, possible memory corruption for param:" << parameterName;
        }
    }
    else if (libParamToWidgetMap.contains(parameterName))
    {
        //All the library parameters
        libParamToWidgetMap.value(parameterName)->setParameterValue(uas,component,parameterName,value);
        if (toolToBoxMap.contains(libParamToWidgetMap.value(parameterName)))
        {
            toolToBoxMap[libParamToWidgetMap.value(parameterName)]->show();
        }
        else
        {
            qCritical() << "Widget with no box, possible memory corruption for param:" << parameterName;
        }
    }
    else
    {
        //Param recieved that we have no metadata for. Search to see if it belongs in a
        //group with some other params
        bool found = false;
        for (int i=0;i<toolWidgets.size();i++)
        {
            if (parameterName.startsWith(toolWidgets[i]->objectName()))
            {
                //It should be grouped with this one, add it.
                toolWidgets[i]->addParam(uas,component,parameterName,value);
                libParamToWidgetMap.insert(parameterName,toolWidgets[i]);
                found  = true;
                break;
            }
        }
        if (!found)
        {
            //New param type, create a QGroupBox for it.
            QWidget* parent;
            if (ui->advancedLeftLayout->count() > ui->advancedRightLayout->count())
            {
                parent = ui->advancedRightContents;
            }
            else
            {
                parent = ui->advancedLeftContents;
            }

            // Create the tool, attaching it to the QGroupBox
            QString tooltitle = parameterName;
            if (parameterName.split("_").size() > 1)
            {
                tooltitle = parameterName.split("_")[0] + "_";
            }
            QGCToolWidget *tool = new QGCToolWidget(tooltitle, tooltitle, parent);
            //tool->setSettings(set);
            libParamToWidgetMap.insert(parameterName,tool);
            toolWidgets.append(tool);
            tool->addParam(uas, component, parameterName, value);
            QGroupBox *box = new QGroupBox(parent);
            box->setTitle(tool->objectName());
            box->setLayout(new QVBoxLayout(box));
            box->layout()->addWidget(tool);

            libParamToWidgetMap.insert(parameterName,tool);
            toolWidgets.append(tool);

            // Make sure we have similar number of widgets on each side.
            if (ui->advancedLeftLayout->count() > ui->advancedRightLayout->count())
            {
                ui->advancedRightLayout->addWidget(box);
            }
            else
            {
                ui->advancedLeftLayout->addWidget(box);
            }
            toolToBoxMap[tool] = box;
        }
    }

    // Channel calibration values
    QRegExp minTpl("RC?_MIN");
    minTpl.setPatternSyntax(QRegExp::Wildcard);
    QRegExp maxTpl("RC?_MAX");
    maxTpl.setPatternSyntax(QRegExp::Wildcard);
    QRegExp trimTpl("RC?_TRIM");
    trimTpl.setPatternSyntax(QRegExp::Wildcard);
    QRegExp revTpl("RC?_REV");
    revTpl.setPatternSyntax(QRegExp::Wildcard);

    // Do not write the RC type, as these values depend on this
    // active onboard parameter

    if (minTpl.exactMatch(parameterName)) {
        bool ok;
        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
        //qDebug() << "PARAM:" << parameterName << "index:" << index;
        if (ok && index < chanMax)
        {
            rcMin[index] = value.toInt();
            updateMinMax();
        }
    }

    if (maxTpl.exactMatch(parameterName)) {
        bool ok;
        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
        if (ok && index < chanMax)
        {
            rcMax[index] = value.toInt();
            updateMinMax();
        }
    }

    if (trimTpl.exactMatch(parameterName)) {
        bool ok;
        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
        if (ok && index < chanMax)
        {
            rcTrim[index] = value.toInt();
        }
    }

    if (revTpl.exactMatch(parameterName)) {
        bool ok;
        unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
        if (ok && index < chanMax)
        {
            rcRev[index] = (value.toInt() == -1) ? true : false;
            updateInvertedCheckboxes(index);
        }
    }

//        mav->setParameter(0, trimTpl.arg(i), rcTrim[i]);
//        mav->setParameter(0, maxTpl.arg(i), rcMax[i]);
//        mav->setParameter(0, revTpl.arg(i), (rcRev[i]) ? -1 : 1);
//    }

    if (rcTypeUpdateRequested > 0 && parameterName == QString("RC_TYPE"))
    {
        rcTypeUpdateRequested = 0;
        updateStatus(tr("Received RC type update, setting parameters based on model."));
        rcType = value.toInt();
        // Request all other parameters as well
        requestCalibrationRC();
    }

    // Order is: roll, pitch, yaw, throttle, mode sw, aux 1-3

    if (parameterName.contains("RC_MAP_ROLL")) {
        rcMapping[0] = value.toInt() - 1;
        ui->rollSpinBox->setValue(rcMapping[0]+1);
        ui->rollSpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_PITCH")) {
        rcMapping[1] = value.toInt() - 1;
        ui->pitchSpinBox->setValue(rcMapping[1]+1);
        ui->pitchSpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_YAW")) {
        rcMapping[2] = value.toInt() - 1;
        ui->yawSpinBox->setValue(rcMapping[2]+1);
        ui->yawSpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_THROTTLE")) {
        rcMapping[3] = value.toInt() - 1;
        ui->throttleSpinBox->setValue(rcMapping[3]+1);
        ui->throttleSpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_MODE_SW")) {
        rcMapping[4] = value.toInt() - 1;
        ui->modeSpinBox->setValue(rcMapping[4]+1);
        ui->modeSpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_AUX1")) {
        rcMapping[5] = value.toInt() - 1;
        ui->aux1SpinBox->setValue(rcMapping[5]+1);
        ui->aux1SpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_AUX2")) {
        rcMapping[6] = value.toInt() - 1;
        ui->aux2SpinBox->setValue(rcMapping[6]+1);
        ui->aux2SpinBox->setEnabled(true);
    }

    if (parameterName.contains("RC_MAP_AUX3")) {
        rcMapping[7] = value.toInt() - 1;
        ui->aux3SpinBox->setValue(rcMapping[7]+1);
        ui->aux3SpinBox->setEnabled(true);
    }

    // Scaling

    if (parameterName.contains("RC_SCALE_ROLL")) {
        rcScaling[0] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_PITCH")) {
        rcScaling[1] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_YAW")) {
        rcScaling[2] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_THROTTLE")) {
        rcScaling[3] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_MODE_SW")) {
        rcScaling[4] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_AUX1")) {
        rcScaling[5] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_AUX2")) {
        rcScaling[6] = value.toFloat();
    }

    if (parameterName.contains("RC_SCALE_AUX3")) {
        rcScaling[7] = value.toFloat();
    }
}

void QGCVehicleConfig::updateStatus(const QString& str)
{
    ui->statusLabel->setText(str);
    ui->statusLabel->setStyleSheet("");
}

void QGCVehicleConfig::updateError(const QString& str)
{
    ui->statusLabel->setText(str);
    ui->statusLabel->setStyleSheet(QString("QLabel { margin: 0px 2px; font: 14px; color: %1; background-color: %2; }").arg(QGC::colorDarkWhite.name()).arg(QGC::colorMagenta.name()));
}
void QGCVehicleConfig::updateMinMax()
{
    // Order is: roll, pitch, yaw, throttle, mode sw, aux 1-3
    /*ui->rollWidget->setMin(rcMin[0]);
    ui->rollWidget->setMax(rcMax[0]);
    ui->pitchWidget->setMin(rcMin[1]);
    ui->pitchWidget->setMax(rcMax[1]);
    ui->yawWidget->setMin(rcMin[2]);
    ui->yawWidget->setMax(rcMax[2]);
    ui->throttleWidget->setMin(rcMin[3]);
    ui->throttleWidget->setMax(rcMax[3]);
    ui->radio5Widget->setMin(rcMin[4]);
    ui->radio5Widget->setMax(rcMax[4]);
    ui->radio6Widget->setMin(rcMin[5]);
    ui->radio6Widget->setMax(rcMax[5]);
    ui->radio7Widget->setMin(rcMin[6]);
    ui->radio7Widget->setMax(rcMax[6]);
    ui->radio8Widget->setMin(rcMin[7]);
    ui->radio8Widget->setMax(rcMax[7]);*/
}

void QGCVehicleConfig::setRCType(int type)
{
    if (!mav) return;

    // XXX TODO Add handling of RC_TYPE vs non-RC_TYPE here

    mav->setParameter(0, "RC_TYPE", type);
    rcTypeUpdateRequested = QGC::groundTimeMilliseconds();
    QTimer::singleShot(rcTypeTimeout+100, this, SLOT(checktimeOuts()));
}

void QGCVehicleConfig::checktimeOuts()
{
    if (rcTypeUpdateRequested > 0)
    {
        if (QGC::groundTimeMilliseconds() - rcTypeUpdateRequested > rcTypeTimeout)
        {
            updateError(tr("Setting remote control timed out - is the system connected?"));
        }
    }
}

void QGCVehicleConfig::updateView()
{
    if (changed)
    {
        if (rc_mode == RC_MODE_1)
        {
            //ui->rollSlider->setValue(rcRoll * 50 + 50);
            //ui->pitchSlider->setValue(rcThrottle * 100);
            //ui->yawSlider->setValue(rcYaw * 50 + 50);
            //ui->throttleSlider->setValue(rcPitch * 50 + 50);
            ui->rollWidget->setValue(rcValue[0]);
            ui->throttleWidget->setValue(rcValue[1]);
            ui->yawWidget->setValue(rcValue[2]);
            ui->pitchWidget->setValue(rcValue[3]);

            ui->rollWidget->setMin(rcMin[0]);
            ui->rollWidget->setMax(rcMax[0]);
            ui->throttleWidget->setMin(rcMin[1]);
            ui->throttleWidget->setMax(rcMax[1]);
            ui->yawWidget->setMin(rcMin[2]);
            ui->yawWidget->setMax(rcMax[2]);
            ui->pitchWidget->setMin(rcMin[3]);
            ui->pitchWidget->setMax(rcMax[3]);
        }
        else if (rc_mode == RC_MODE_2)
        {
            //ui->rollSlider->setValue(rcRoll * 50 + 50);
            //ui->pitchSlider->setValue(rcPitch * 50 + 50);
            //ui->yawSlider->setValue(rcYaw * 50 + 50);
            //ui->throttleSlider->setValue(rcThrottle * 100);
            ui->rollWidget->setValue(rcValue[0]);
            ui->pitchWidget->setValue(rcValue[1]);
            ui->yawWidget->setValue(rcValue[2]);
            ui->throttleWidget->setValue(rcValue[3]);

            ui->rollWidget->setMin(rcMin[0]);
            ui->rollWidget->setMax(rcMax[0]);
            ui->pitchWidget->setMin(rcMin[1]);
            ui->pitchWidget->setMax(rcMax[1]);
            ui->yawWidget->setMin(rcMin[2]);
            ui->yawWidget->setMax(rcMax[2]);
            ui->throttleWidget->setMin(rcMin[3]);
            ui->throttleWidget->setMax(rcMax[3]);
        }
        else if (rc_mode == RC_MODE_3)
        {
            //ui->rollSlider->setValue(rcYaw * 50 + 50);
            //ui->pitchSlider->setValue(rcThrottle * 100);
            //ui->yawSlider->setValue(rcRoll * 50 + 50);
            //ui->throttleSlider->setValue(rcPitch * 50 + 50);
            ui->yawWidget->setValue(rcValue[0]);
            ui->throttleWidget->setValue(rcValue[1]);
            ui->rollWidget->setValue(rcValue[2]);
            ui->pitchWidget->setValue(rcValue[3]);

            ui->yawWidget->setMin(rcMin[0]);
            ui->yawWidget->setMax(rcMax[0]);
            ui->throttleWidget->setMin(rcMin[1]);
            ui->throttleWidget->setMax(rcMax[1]);
            ui->rollWidget->setMin(rcMin[2]);
            ui->rollWidget->setMax(rcMax[2]);
            ui->pitchWidget->setMin(rcMin[3]);
            ui->pitchWidget->setMax(rcMax[3]);
        }
        else if (rc_mode == RC_MODE_4)
        {
            //ui->rollSlider->setValue(rcYaw * 50 + 50);
            //ui->pitchSlider->setValue(rcPitch * 50 + 50);
            //ui->yawSlider->setValue(rcRoll * 50 + 50);
            //ui->throttleSlider->setValue(rcThrottle * 100);
            ui->yawWidget->setValue(rcValue[0]);
            ui->pitchWidget->setValue(rcValue[1]);
            ui->rollWidget->setValue(rcValue[2]);
            ui->throttleWidget->setValue(rcValue[3]);

            ui->yawWidget->setMin(rcMin[0]);
            ui->yawWidget->setMax(rcMax[0]);
            ui->pitchWidget->setMin(rcMin[1]);
            ui->pitchWidget->setMax(rcMax[1]);
            ui->rollWidget->setMin(rcMin[2]);
            ui->rollWidget->setMax(rcMax[2]);
            ui->throttleWidget->setMin(rcMin[3]);
            ui->throttleWidget->setMax(rcMax[3]);
        }
        else if (rc_mode == RC_MODE_NONE)
        {
            ui->rollWidget->setValue(rcValue[0]);
            ui->pitchWidget->setValue(rcValue[1]);
            ui->throttleWidget->setValue(rcValue[2]);
            ui->yawWidget->setValue(rcValue[3]);

            ui->rollWidget->setMin(800);
            ui->rollWidget->setMax(2200);
            ui->pitchWidget->setMin(800);
            ui->pitchWidget->setMax(2200);
            ui->throttleWidget->setMin(800);
            ui->throttleWidget->setMax(2200);
            ui->yawWidget->setMin(800);
            ui->yawWidget->setMax(2200);
        }

        ui->chanLabel->setText(QString("%1/%2").arg(rcValue[rcMapping[0]]).arg(rcRoll, 5, 'f', 2, QChar(' ')));
        ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[rcMapping[1]]).arg(rcPitch, 5, 'f', 2, QChar(' ')));
        ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[rcMapping[2]]).arg(rcYaw, 5, 'f', 2, QChar(' ')));
        ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[rcMapping[3]]).arg(rcThrottle, 5, 'f', 2, QChar(' ')));



        //ui->modeSwitchSlider->setValue(rcMode * 50 + 50);
        ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[rcMapping[4]]).arg(rcMode, 5, 'f', 2, QChar(' ')));

        if (rcValue[rcMapping[4] != UINT16_MAX]) {
            ui->radio5Widget->setValue(rcValue[4]);
            ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[rcMapping[5]]).arg(rcAux1, 5, 'f', 2, QChar(' ')));
        } else {
            ui->chanLabel_5->setText(tr("---"));
        }

        if (rcValue[rcMapping[5]] != UINT16_MAX) {
            //ui->aux1Slider->setValue(rcAux1 * 50 + 50);
            ui->radio6Widget->setValue(rcValue[5]);
            ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[rcMapping[5]]).arg(rcAux1, 5, 'f', 2, QChar(' ')));
        } else {
            ui->chanLabel_6->setText(tr("---"));
        }

        if (rcValue[rcMapping[6]] != UINT16_MAX) {
            //ui->aux2Slider->setValue(rcAux2 * 50 + 50);
            ui->radio7Widget->setValue(rcValue[6]);
            ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[rcMapping[6]]).arg(rcAux2, 5, 'f', 2, QChar(' ')));
        } else {
            ui->chanLabel_7->setText(tr("---"));
        }

        if (rcValue[rcMapping[7]] != UINT16_MAX) {
            //ui->aux3Slider->setValue(rcAux3 * 50 + 50);
            ui->radio8Widget->setValue(rcValue[7]);
            ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[rcMapping[7]]).arg(rcAux3, 5, 'f', 2, QChar(' ')));
        } else {
            ui->chanLabel_8->setText(tr("---"));
        }

        changed = false;
    }
}