From 47d59fa94e7b97df58ff60d7031ee1246564e351 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 24 Nov 2012 19:11:37 +0100
Subject: [PATCH] Firmware upgrade app, XPlane link improvements, better RC
 calibration

---
 src/PX4FirmwareUpgradeWorker.cc         |  64 +++--
 src/PX4FirmwareUpgradeWorker.h          |  23 +-
 src/apps/qupgrade/QUpgradeApp.cc        |  16 +-
 src/apps/qupgrade/QUpgradeMainWindow.cc |   4 -
 src/apps/qupgrade/QUpgradeMainWindow.h  |   2 -
 src/apps/qupgrade/QUpgradeMainWindow.ui |  10 +-
 src/comm/QGCXPlaneLink.cc               |  21 +-
 src/comm/QGCXPlaneLink.h                |   3 +
 src/ui/PX4FirmwareUpgrader.cc           |  17 +-
 src/ui/QGCHilConfiguration.cc           |  10 +-
 src/ui/QGCHilConfiguration.ui           |  53 ++--
 src/ui/QGCVehicleConfig.cc              | 334 ++++++++++++++----------
 src/ui/QGCVehicleConfig.h               |  78 +++++-
 src/ui/QGCVehicleConfig.ui              |  57 ++--
 14 files changed, 447 insertions(+), 245 deletions(-)

diff --git a/src/PX4FirmwareUpgradeWorker.cc b/src/PX4FirmwareUpgradeWorker.cc
index 461b82d7e..b4a6755de 100644
--- a/src/PX4FirmwareUpgradeWorker.cc
+++ b/src/PX4FirmwareUpgradeWorker.cc
@@ -1,3 +1,6 @@
+#include <QJsonDocument>
+#include <QFile>
+
 #include "PX4FirmwareUpgradeWorker.h"
 
 #include <SerialLink.h>
@@ -49,21 +52,28 @@ PX4FirmwareUpgradeWorker* PX4FirmwareUpgradeWorker::putWorkerInThread(QObject *p
 
     // Starts an event loop, and emits workerThread->started()
     workerThread->start();
+    return worker;
 }
 
 
-bool PX4FirmwareUpgradeWorker::startContinousScan()
+void PX4FirmwareUpgradeWorker::startContinousScan()
 {
-    while (true) {
-        if (detect()) {
-            break;
-        }
+    exitThread = false;
+    while (!exitThread) {
+//        if (detect()) {
+//            break;
+//        }
+        QGC::SLEEP::msleep(20);
     }
 
-    return true;
+    if (exitThread) {
+        link->disconnect();
+        delete link;
+        exit(0);
+    }
 }
 
-bool PX4FirmwareUpgradeWorker::detect()
+void PX4FirmwareUpgradeWorker::detect()
 {
     if (!link)
     {
@@ -80,7 +90,7 @@ bool PX4FirmwareUpgradeWorker::detect()
         // Ignore known wrong link names
 
         if (ports->at(i).contains("Bluetooth")) {
-            continue;
+            //continue;
         }
 
         link->setPortName(ports->at(i));
@@ -101,12 +111,6 @@ bool PX4FirmwareUpgradeWorker::detect()
             break;
     }
 
-    if (insync) {
-        return true;
-    } else {
-        return false;
-    }
-
     //ui.portName->setCurrentIndex(ui.baudRate->findText(QString("%1").arg(this->link->getPortName())));
 
     // Set port
@@ -119,11 +123,15 @@ void PX4FirmwareUpgradeWorker::receiveBytes(LinkInterface* link, QByteArray b)
 {
     for (int position = 0; position < b.size(); position++) {
         qDebug() << "BYTES";
-        qDebug() << std::hex << (char)(b[position]);
+        qDebug() << (char)(b[position]);
         if (((const char)b[position]) == INSYNC)
         {
             qDebug() << "SYNC";
             insync = true;
+        }
+
+        if (insync && ((const char)b[position]) == OK)
+        {
             emit detectionStatusChanged("Found PX4 board");
         }
     }
@@ -131,7 +139,31 @@ void PX4FirmwareUpgradeWorker::receiveBytes(LinkInterface* link, QByteArray b)
     printf("\n");
 }
 
-bool PX4FirmwareUpgradeWorker::upgrade(const QString &filename)
+void PX4FirmwareUpgradeWorker::loadFirmware(const QString &filename)
+{
+    qDebug() << __FILE__ << __LINE__ << "LOADING FW";
+    QFile f(filename);
+    if (f.open(QIODevice::ReadOnly))
+    {
+        QByteArray buf = f.readAll();
+        f.close();
+        firmware = QJsonDocument::fromBinaryData(buf);
+        if (firmware.isNull()) {
+            emit upgradeStatusChanged(tr("Failed decoding file %1").arg(filename));
+        } else {
+            emit upgradeStatusChanged(tr("Ready to flash %1").arg(filename));
+        }
+    } else {
+        emit upgradeStatusChanged(tr("Failed opening file %1").arg(filename));
+    }
+}
+
+void PX4FirmwareUpgradeWorker::upgrade()
 {
+    emit upgradeStatusChanged(tr("Starting firmware upgrade.."));
+}
 
+void PX4FirmwareUpgradeWorker::abort()
+{
+    exitThread = true;
 }
diff --git a/src/PX4FirmwareUpgradeWorker.h b/src/PX4FirmwareUpgradeWorker.h
index 88c1acda3..873eadc35 100644
--- a/src/PX4FirmwareUpgradeWorker.h
+++ b/src/PX4FirmwareUpgradeWorker.h
@@ -2,6 +2,7 @@
 #define PX4FIRMWAREUPGRADEWORKER_H
 
 #include <QObject>
+#include <QJsonDocument>
 
 #include <SerialLink.h>
 
@@ -24,7 +25,7 @@ public slots:
      * @brief Continously scan for bootloaders
      * @return
      */
-    bool startContinousScan();
+    void startContinousScan();
 
     /**
      * @brief Detect connected PX4 bootloaders
@@ -34,14 +35,19 @@ public slots:
      *
      * @return true if found on one link, false else
      */
-    bool detect();
+    void detect();
 
     /**
      * @brief Upgrade the firmware using the currently connected link
      * @param filename file name / path of the firmware file
-     * @return true if upgrade succeeds, false else
      */
-    bool upgrade(const QString &filename);
+    void upgrade();
+
+    /**
+     * @brief Load firmware from disk into memory
+     * @param filename
+     */
+    void loadFirmware(const QString &filename);
 
     /**
      * @brief Receive bytes from a link
@@ -50,9 +56,18 @@ public slots:
      */
     void receiveBytes(LinkInterface* link, QByteArray b);
 
+    /**
+     * @brief Abort upgrade worker
+     */
+    void abort();
+
+protected:
+    bool exitThread;
+
 private:
     SerialLink *link;
     bool insync;
+    QJsonDocument firmware;
 };
 
 #endif // PX4FIRMWAREUPGRADEWORKER_H
diff --git a/src/apps/qupgrade/QUpgradeApp.cc b/src/apps/qupgrade/QUpgradeApp.cc
index 1ca955b3a..558845aeb 100644
--- a/src/apps/qupgrade/QUpgradeApp.cc
+++ b/src/apps/qupgrade/QUpgradeApp.cc
@@ -72,13 +72,21 @@ QUpgradeApp::QUpgradeApp(int &argc, char* argv[]) : QApplication(argc, argv)
 
     // Create main window
     QUpgradeMainWindow* window = new QUpgradeMainWindow();
+    PX4FirmwareUpgrader *upgrader = new PX4FirmwareUpgrader(window);
+    window->setCentralWidget(upgrader);
 
     // Get PX4 upgrade widget and instantiate worker thread
     PX4FirmwareUpgradeWorker* worker = PX4FirmwareUpgradeWorker::putWorkerInThread(this);
-    connect(worker, SIGNAL(detectionStatusChanged(QString)), window->firmwareUpgrader(), SLOT(setDetectionStatusText(QString)));
-    connect(worker, SIGNAL(upgradeStatusChanged(QString)), window->firmwareUpgrader(), SLOT(setFlashStatusText(QString)));
-    connect(worker, SIGNAL(upgradeProgressChanged(int)), window->firmwareUpgrader(), SLOT(setFlashProgress(int)));
-    connect(worker, SIGNAL(validPortFound(QString)), window->firmwareUpgrader(), SLOT(setPortName(QString)));
+
+    connect(worker, SIGNAL(detectionStatusChanged(QString)), upgrader, SLOT(setDetectionStatusText(QString)), Qt::QueuedConnection);
+    connect(worker, SIGNAL(upgradeStatusChanged(QString)), upgrader, SLOT(setFlashStatusText(QString)), Qt::QueuedConnection);
+    connect(worker, SIGNAL(upgradeProgressChanged(int)), upgrader, SLOT(setFlashProgress(int)), Qt::QueuedConnection);
+    connect(worker, SIGNAL(validPortFound(QString)), upgrader, SLOT(setPortName(QString)));
+    connect(upgrader, SIGNAL(firmwareFileNameSet(QString)), worker, SLOT(loadFirmware(QString)), Qt::QueuedConnection);
+    connect(upgrader, SIGNAL(upgrade()), worker, SLOT(upgrade()), Qt::QueuedConnection);
+    connect(this, SIGNAL(lastWindowClosed()), worker, SLOT(abort()), Qt::QueuedConnection);
+
+    worker->loadFirmware("abc");
 
     window->setWindowTitle(applicationName() + " " + applicationVersion());
     window->show();
diff --git a/src/apps/qupgrade/QUpgradeMainWindow.cc b/src/apps/qupgrade/QUpgradeMainWindow.cc
index ad0bb2d18..fea3841df 100644
--- a/src/apps/qupgrade/QUpgradeMainWindow.cc
+++ b/src/apps/qupgrade/QUpgradeMainWindow.cc
@@ -42,10 +42,6 @@ QUpgradeMainWindow::QUpgradeMainWindow(QWidget *parent) :
     ui->setupUi(this);
 }
 
-PX4FirmwareUpgrader* QUpgradeMainWindow::firmwareUpgrader() {
-    return ui->centralwidget;
-}
-
 QUpgradeMainWindow::~QUpgradeMainWindow()
 {
     delete ui;
diff --git a/src/apps/qupgrade/QUpgradeMainWindow.h b/src/apps/qupgrade/QUpgradeMainWindow.h
index dbb523779..c012600dc 100644
--- a/src/apps/qupgrade/QUpgradeMainWindow.h
+++ b/src/apps/qupgrade/QUpgradeMainWindow.h
@@ -48,8 +48,6 @@ public:
     explicit QUpgradeMainWindow(QWidget *parent = 0);
     ~QUpgradeMainWindow();
 
-    PX4FirmwareUpgrader* firmwareUpgrader();
-
 public slots:
 
 protected:
diff --git a/src/apps/qupgrade/QUpgradeMainWindow.ui b/src/apps/qupgrade/QUpgradeMainWindow.ui
index 75f16d006..5c806bbff 100644
--- a/src/apps/qupgrade/QUpgradeMainWindow.ui
+++ b/src/apps/qupgrade/QUpgradeMainWindow.ui
@@ -13,7 +13,7 @@
   <property name="windowTitle">
    <string>MainWindow</string>
   </property>
-  <widget class="PX4FirmwareUpgrader" name="centralwidget">
+  <widget class="QWidget" name="centralwidget">
    <layout class="QGridLayout" name="gridLayout" rowstretch="0"/>
   </widget>
   <widget class="QMenuBar" name="menubar">
@@ -28,14 +28,6 @@
   </widget>
   <widget class="QStatusBar" name="statusbar"/>
  </widget>
- <customwidgets>
-  <customwidget>
-   <class>PX4FirmwareUpgrader</class>
-   <extends>QWidget</extends>
-   <header location="global">PX4FirmwareUpgrader.h</header>
-   <container>1</container>
-  </customwidget>
- </customwidgets>
  <resources/>
  <connections/>
 </ui>
diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index 165715291..521beb5b6 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -49,7 +49,10 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     terraSync(NULL),
     airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
     xPlaneConnected(false),
-    xPlaneVersion(0)
+    xPlaneVersion(0),
+    simUpdateLast(QGC::groundTimeMilliseconds()),
+    simUpdateLastText(QGC::groundTimeMilliseconds()),
+    simUpdateHz(0)
 {
     this->localHost = localHost;
     this->localPort = localPort/*+mav->getUASID()*/;
@@ -374,7 +377,7 @@ void QGCXPlaneLink::readBytes()
     // Only emit updates on attitude message
     bool emitUpdate = false;
 
-    const qint64 maxLength = 65536;
+    const qint64 maxLength = 1000;
     char data[maxLength];
     QHostAddress sender;
     quint16 senderPort;
@@ -509,8 +512,18 @@ void QGCXPlaneLink::readBytes()
     }
 
     // Send updated state
-    if (emitUpdate)
+    if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 3)
     {
+        simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
+        if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
+            emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
+            // Reset lowpass with current value
+            simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
+            // Set state
+            simUpdateLastText = QGC::groundTimeMilliseconds();
+        }
+        simUpdateLast = QGC::groundTimeMilliseconds();
+
         emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                          pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
                          vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
@@ -518,7 +531,7 @@ void QGCXPlaneLink::readBytes()
 
     if (!oldConnectionState && xPlaneConnected)
     {
-        emit statusMessage("Receiving from XPlane.");
+        emit statusMessage(tr("Receiving from XPlane."));
     }
 
     //    // Echo data for debugging purposes
diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h
index e389b7b8f..d89e98763 100644
--- a/src/comm/QGCXPlaneLink.h
+++ b/src/comm/QGCXPlaneLink.h
@@ -190,6 +190,9 @@ protected:
     enum AIRFRAME airframeID;
     bool xPlaneConnected;
     unsigned int xPlaneVersion;
+    quint64 simUpdateLast;
+    quint64 simUpdateLastText;
+    float simUpdateHz;
 
     void setName(QString name);
 
diff --git a/src/ui/PX4FirmwareUpgrader.cc b/src/ui/PX4FirmwareUpgrader.cc
index 5d9fabff8..c2ae3fae8 100644
--- a/src/ui/PX4FirmwareUpgrader.cc
+++ b/src/ui/PX4FirmwareUpgrader.cc
@@ -1,9 +1,14 @@
+#include <QFileDialog>
+#include <QDesktopServices>
+#include <QSettings>
+
 #include "PX4FirmwareUpgrader.h"
 #include "ui_PX4FirmwareUpgrader.h"
 
 #include <QGC.h>
 #include <QDebug>
 
+
 PX4FirmwareUpgrader::PX4FirmwareUpgrader(QWidget *parent) :
     QWidget(parent),
     ui(new Ui::PX4FirmwareUpgrader)
@@ -21,7 +26,16 @@ PX4FirmwareUpgrader::~PX4FirmwareUpgrader()
 
 void PX4FirmwareUpgrader::selectFirmwareFile()
 {
-
+    QSettings settings;
+    QString path = settings.value("PX4_FIRMWARE_PATH",
+                                     QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)).toString();
+    const QString widgetFileExtension(".px4");
+    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify File Name"),
+                       path,
+                       tr("PX4 Firmware (*%1);;").arg(widgetFileExtension));
+    settings.setValue("PX4_FIRMWARE_PATH", fileName);
+    qDebug() << "EMITTING SIGNAL";
+    emit firmwareFileNameSet(fileName);
 }
 
 void PX4FirmwareUpgrader::setDetectionStatusText(const QString &text)
@@ -32,6 +46,7 @@ void PX4FirmwareUpgrader::setDetectionStatusText(const QString &text)
 void PX4FirmwareUpgrader::setFlashStatusText(const QString &text)
 {
     ui->flashProgressLabel->setText(text);
+    qDebug() << __FILE__ << __LINE__ << "LABEL" << text;
 }
 
 void PX4FirmwareUpgrader::setFlashProgress(int percent)
diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc
index a83397ce1..d6253cefb 100644
--- a/src/ui/QGCHilConfiguration.cc
+++ b/src/ui/QGCHilConfiguration.cc
@@ -30,7 +30,12 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index)
         mav->enableHilFlightGear(false, "");
         QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(mav, this);
         hfgconf->show();
-        ui->simulatorConfigurationDockWidget->setWidget(hfgconf);
+        ui->simulatorConfigurationLayout->addWidget(hfgconf);
+        QGCFlightGearLink* fg = dynamic_cast<QGCFlightGearLink*>(mav->getHILSimulation());
+        if (fg)
+        {
+            connect(fg, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString)));
+        }
 
     }
     else if(2 == index || 3 == index)
@@ -39,13 +44,14 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index)
         mav->enableHilXPlane(false);
         QGCHilXPlaneConfiguration* hxpconf = new QGCHilXPlaneConfiguration(mav->getHILSimulation(), this);
         hxpconf->show();
-        ui->simulatorConfigurationDockWidget->setWidget(hxpconf);
+        ui->simulatorConfigurationLayout->addWidget(hxpconf);
 
         // Select correct version of XPlane
         QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(mav->getHILSimulation());
         if (xplane)
         {
             xplane->setVersion((index == 2) ? 10 : 9);
+            connect(xplane, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString)));
         }
     }
 }
diff --git a/src/ui/QGCHilConfiguration.ui b/src/ui/QGCHilConfiguration.ui
index ace311076..da8adaf27 100644
--- a/src/ui/QGCHilConfiguration.ui
+++ b/src/ui/QGCHilConfiguration.ui
@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>305</width>
-    <height>355</height>
+    <width>366</width>
+    <height>301</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -19,22 +19,22 @@
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" columnstretch="40,0,0">
-   <item row="0" column="0">
-    <widget class="QLabel" name="simLabel">
+  <layout class="QGridLayout" name="gridLayout" rowstretch="1,100,1" columnstretch="40,0">
+   <item row="2" column="0">
+    <widget class="QLabel" name="statusLabel">
      <property name="text">
-      <string>Simulator</string>
+      <string>Status</string>
      </property>
     </widget>
    </item>
-   <item row="3" column="0">
-    <widget class="QLabel" name="statusLabel">
+   <item row="0" column="0">
+    <widget class="QLabel" name="simLabel">
      <property name="text">
-      <string>Status</string>
+      <string>Simulator</string>
      </property>
     </widget>
    </item>
-   <item row="0" column="2">
+   <item row="0" column="1">
     <widget class="QComboBox" name="simComboBox">
      <property name="editable">
       <bool>true</bool>
@@ -61,35 +61,12 @@
      </item>
     </widget>
    </item>
-   <item row="2" column="0" colspan="3">
-    <widget class="QDockWidget" name="simulatorConfigurationDockWidget">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="minimumSize">
-      <size>
-       <width>20</width>
-       <height>150</height>
-      </size>
-     </property>
-     <property name="features">
-      <set>QDockWidget::NoDockWidgetFeatures</set>
+   <item row="1" column="0" colspan="2">
+    <layout class="QVBoxLayout" name="simulatorConfigurationLayout">
+     <property name="spacing">
+      <number>0</number>
      </property>
-     <property name="windowTitle">
-      <string>Simulator Options</string>
-     </property>
-     <widget class="QWidget" name="dockWidgetContents">
-      <property name="sizePolicy">
-       <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
-        <horstretch>0</horstretch>
-        <verstretch>0</verstretch>
-       </sizepolicy>
-      </property>
-     </widget>
-    </widget>
+    </layout>
    </item>
   </layout>
  </widget>
diff --git a/src/ui/QGCVehicleConfig.cc b/src/ui/QGCVehicleConfig.cc
index 318ef8d75..39c421fcb 100644
--- a/src/ui/QGCVehicleConfig.cc
+++ b/src/ui/QGCVehicleConfig.cc
@@ -11,6 +11,7 @@
 QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
     QWidget(parent),
     mav(NULL),
+    chanCount(0),
     changed(true),
     rc_mode(RC_MODE_2),
     rcRoll(0.0f),
@@ -21,6 +22,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
     rcAux1(0.0f),
     rcAux2(0.0f),
     rcAux3(0.0f),
+    calibrationEnabled(false),
     ui(new Ui::QGCVehicleConfig)
 {
     setObjectName("QGC_VEHICLECONFIG");
@@ -31,9 +33,31 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
 
     ui->rcModeComboBox->setCurrentIndex((int)rc_mode - 1);
 
+    ui->rcCalibrationButton->setCheckable(true);
     connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool)));
     connect(ui->storeButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
     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*)));
 
@@ -41,7 +65,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
 
     for (unsigned int i = 0; i < chanMax; i++)
     {
-        rcValue[i] = 1500;
+        rcValue[i] = UINT16_MAX;
     }
 
     updateTimer.setInterval(150);
@@ -75,15 +99,35 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
     }
 }
 
+void QGCVehicleConfig::setTrimPositions()
+{
+    // Set trim for roll, pitch, yaw, throttle
+    rcTrim[rcMapping[0]] = rcValue[0]; // roll
+    rcTrim[rcMapping[1]] = rcValue[1]; // pitch
+    rcTrim[rcMapping[2]] = rcValue[2]; // yaw
+    rcTrim[rcMapping[3]] = rcMin[3];   // throttle
+    rcTrim[rcMapping[4]] = ((rcMax[4] - rcMin[4]) / 2.0f) + rcMin[4];   // mode sw
+    rcTrim[rcMapping[5]] = ((rcMax[5] - rcMin[5]) / 2.0f) + rcMin[5];   // aux 1
+    rcTrim[rcMapping[5]] = ((rcMax[6] - rcMin[6]) / 2.0f) + rcMin[6];   // aux 2
+    rcTrim[rcMapping[5]] = ((rcMax[7] - rcMin[7]) / 2.0f) + rcMin[7];   // aux 3
+}
+
+void QGCVehicleConfig::detectChannelInversion()
+{
+
+}
+
 void QGCVehicleConfig::startCalibrationRC()
 {
     ui->rcTypeComboBox->setEnabled(false);
     ui->rcCalibrationButton->setText(tr("Stop RC Calibration"));
     resetCalibrationRC();
+    calibrationEnabled = true;
 }
 
 void QGCVehicleConfig::stopCalibrationRC()
 {
+    calibrationEnabled = false;
     ui->rcTypeComboBox->setEnabled(true);
     ui->rcCalibrationButton->setText(tr("Start RC Calibration"));
 }
@@ -111,6 +155,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
     // Reset current state
     resetCalibrationRC();
 
+    chanCount = 0;
+
     // Connect new system
     mav = active;
     connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
@@ -130,13 +176,18 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
     {
         toolWidgets.append(tool);
         ui->sensorLayout->addWidget(tool);
+    } else {
+        delete tool;
     }
+
     // Load multirotor attitude pid
     tool = new QGCToolWidget("", this);
     if (tool->loadSettings(defaultsDir + "px4_mc_attitude_pid_params.qgw", false))
     {
         toolWidgets.append(tool);
         ui->multiRotorAttitudeLayout->addWidget(tool);
+    } else {
+        delete tool;
     }
 
     // Load multirotor position pid
@@ -145,6 +196,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
     {
         toolWidgets.append(tool);
         ui->multiRotorPositionLayout->addWidget(tool);
+    } else {
+        delete tool;
     }
 
     // Load fixed wing attitude pid
@@ -153,6 +206,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
     {
         toolWidgets.append(tool);
         ui->fixedWingAttitudeLayout->addWidget(tool);
+    } else {
+        delete tool;
     }
 
     // Load fixed wing position pid
@@ -161,6 +216,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
     {
         toolWidgets.append(tool);
         ui->fixedWingPositionLayout->addWidget(tool);
+    } else {
+        delete tool;
     }
 
     updateStatus(QString("Reading from system %1").arg(mav->getUASName()));
@@ -190,23 +247,36 @@ void QGCVehicleConfig::writeCalibrationRC()
     // Do not write the RC type, as these values depend on this
     // active onboard parameter
 
-    for (unsigned int i = 0; i < chanMax; ++i)
+    for (unsigned int i = 0; i < chanCount; ++i)
     {
+        qDebug() << "SENDING" << minTpl.arg(i+1) << rcMin[i];
         mav->setParameter(0, minTpl.arg(i+1), rcMin[i]);
+        QGC::SLEEP::usleep(50000);
         mav->setParameter(0, trimTpl.arg(i+1), rcTrim[i]);
+        QGC::SLEEP::usleep(50000);
         mav->setParameter(0, maxTpl.arg(i+1), rcMax[i]);
-        mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1 : 1);
+        QGC::SLEEP::usleep(50000);
+        mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1.0f : 1.0f);
+        QGC::SLEEP::usleep(50000);
     }
 
     // Write mappings
-    mav->setParameter(0, "RC_MAP_ROLL", rcMapping[0]);
-    mav->setParameter(0, "RC_MAP_PITCH", rcMapping[1]);
-    mav->setParameter(0, "RC_MAP_THROTTLE", rcMapping[2]);
-    mav->setParameter(0, "RC_MAP_YAW", rcMapping[3]);
-    mav->setParameter(0, "RC_MAP_MODE_SW", rcMapping[4]);
-    mav->setParameter(0, "RC_MAP_AUX1", rcMapping[5]);
-    mav->setParameter(0, "RC_MAP_AUX2", rcMapping[6]);
-    mav->setParameter(0, "RC_MAP_AUX3", rcMapping[7]);
+    mav->setParameter(0, "RC_MAP_ROLL", (int32_t)(rcMapping[0]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[2]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[3]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_AUX1", (int32_t)(rcMapping[5]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_AUX2", (int32_t)(rcMapping[6]+1));
+    QGC::SLEEP::usleep(50000);
+    mav->setParameter(0, "RC_MAP_AUX3", (int32_t)(rcMapping[7]+1));
+    QGC::SLEEP::usleep(50000);
 }
 
 void QGCVehicleConfig::requestCalibrationRC()
@@ -238,126 +308,106 @@ void QGCVehicleConfig::writeParameters()
 {
     updateStatus(tr("Writing all onboard parameters."));
     writeCalibrationRC();
+    mav->writeParametersToStorage();
 }
 
 void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
 {
-//    /* scale around the mid point differently for lower and upper range */
-//            if (ppm_buffer[i] > _rc.chan[i].mid + _parameters.dz[i]) {
-//                _rc.chan[i].scaled = ((ppm_buffer[i] - _parameters.trim[i]) / (_parameters.max[i] - _parameters.trim[i]));
-//            } else if ((ppm_buffer[i] < _rc_chan[i].mid - _parameters.dz[i])) {
-//                _rc.chan[i].scaled = -1.0 + ((ppm_buffer[i] - _parameters.min[i]) / (_parameters.trim[i] - _parameters.min[i]));
-//            } else {
-//                /* in the configured dead zone, output zero */
-//                _rc.chan[i].scaled = 0.0f;
-//            }
-
-    if (chan < 0 || static_cast<unsigned int>(chan) >= chanMax)
+    // Check if index and values are sane
+    if (chan < 0 || static_cast<unsigned int>(chan) >= chanMax || val < 500 || val > 2500)
         return;
 
-    if (val < rcMin[chan])
-    {
-        rcMin[chan] = val;
+    if (chan + 1 > chanCount) {
+        chanCount = chan+1;
+    }
+
+    // Update calibration data
+    if (calibrationEnabled) {
+        if (val < rcMin[chan])
+        {
+            rcMin[chan] = val;
+        }
+
+        if (val > rcMax[chan])
+        {
+            rcMax[chan] = val;
+        }
     }
 
-    if (val > rcMax[chan])
+    // Normalized value
+    float normalized;
+
+    if (val >= rcTrim[chan])
     {
-        rcMax[chan] = val;
+        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
-        if (rcRoll >= rcTrim[chan])
-        {
-            rcRoll = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
-        }
-        else
-        {
-            rcRoll = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
-        }
-
         rcValue[0] = val;
-        rcRoll = qBound(-1.0f, rcRoll, 1.0f);
+        rcRoll = normalized;
     }
     else if (chan == rcMapping[1])
     {
         // PITCH
-        if (rcPitch >= rcTrim[chan])
-        {
-            rcPitch = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
-        }
-        else
-        {
-            rcPitch = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
-        }
         rcValue[1] = val;
-        rcPitch = qBound(-1.0f, rcPitch, 1.0f);
+        rcPitch = normalized;
     }
     else if (chan == rcMapping[2])
     {
-        // YAW
-        if (rcYaw >= rcTrim[chan])
-        {
-            rcYaw = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
-        }
-        else
-        {
-            rcYaw = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
-        }
         rcValue[2] = val;
-        rcYaw = qBound(-1.0f, rcYaw, 1.0f);
+        rcYaw = normalized;
     }
     else if (chan == rcMapping[3])
     {
         // THROTTLE
-        if (rcThrottle >= rcTrim[chan])
-        {
-            rcThrottle = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
-        }
-        else
-        {
-            rcThrottle = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
+        if (rcRev[chan]) {
+            rcThrottle = 1.0f + normalized;
+        } else {
+            rcThrottle = normalized;
         }
+
         rcValue[3] = val;
-        rcThrottle = qBound(-1.0f, rcThrottle, 1.0f);
+        rcThrottle = qBound(0.0f, rcThrottle, 1.0f);
     }
     else if (chan == rcMapping[4])
     {
         // MODE SWITCH
-        if (rcMode >= rcTrim[chan])
-        {
-            rcMode = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]);
-        }
-        else
-        {
-            rcMode = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]);
-        }
+        rcMode = normalized;
         rcValue[4] = val;
-        rcMode = qBound(-1.0f, rcMode, 1.0f);
     }
     else if (chan == rcMapping[5])
     {
         // AUX1
-        rcAux1 = val;
+        rcAux1 = normalized;
         rcValue[5] = val;
     }
     else if (chan == rcMapping[6])
     {
         // AUX2
-        rcAux2 = val;
+        rcAux2 = normalized;
         rcValue[6] = val;
     }
     else if (chan == rcMapping[7])
     {
         // AUX3
-        rcAux3 = val;
+        rcAux3 = normalized;
         rcValue[7] = val;
     }
 
     changed = true;
 
-    //qDebug() << "RC CHAN:" << chan << "PPM:" << val;
+    //qDebug() << "RC CHAN:" << chan << "PPM:" << val << "NORMALIZED:" << normalized;
 }
 
 void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
@@ -381,7 +431,8 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
     if (minTpl.exactMatch(parameterName)) {
         bool ok;
         unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
-        if (ok && (index > 0) && (index < chanMax))
+        qDebug() << "PARAM:" << parameterName << "index:" << index;
+        if (ok && (index >= 0) && (index < chanMax))
         {
             rcMin[index] = value.toInt();
         }
@@ -390,7 +441,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
     if (maxTpl.exactMatch(parameterName)) {
         bool ok;
         unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
-        if (ok && (index > 0) && (index < chanMax))
+        if (ok && (index >= 0) && (index < chanMax))
         {
             rcMax[index] = value.toInt();
         }
@@ -399,7 +450,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
     if (trimTpl.exactMatch(parameterName)) {
         bool ok;
         unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
-        if (ok && (index > 0) && (index < chanMax))
+        if (ok && (index >= 0) && (index < chanMax))
         {
             rcTrim[index] = value.toInt();
         }
@@ -408,7 +459,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
     if (revTpl.exactMatch(parameterName)) {
         bool ok;
         unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
-        if (ok && (index > 0) && (index < chanMax))
+        if (ok && (index >= 0) && (index < chanMax))
         {
             rcRev[index] = (value.toInt() == -1) ? true : false;
 
@@ -461,77 +512,77 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
     // Order is: roll, pitch, yaw, throttle, mode sw, aux 1-3
 
     if (parameterName.contains("RC_MAP_ROLL")) {
-        rcMapping[0] = value.toInt();
-        ui->rollSpinBox->setValue(rcMapping[0]);
+        rcMapping[0] = value.toInt() - 1;
+        ui->rollSpinBox->setValue(rcMapping[0]+1);
     }
 
     if (parameterName.contains("RC_MAP_PITCH")) {
-        rcMapping[1] = value.toInt();
-        ui->pitchSpinBox->setValue(rcMapping[1]);
+        rcMapping[1] = value.toInt() - 1;
+        ui->pitchSpinBox->setValue(rcMapping[1]+1);
     }
 
     if (parameterName.contains("RC_MAP_YAW")) {
-        rcMapping[2] = value.toInt();
-        ui->yawSpinBox->setValue(rcMapping[2]);
+        rcMapping[2] = value.toInt() - 1;
+        ui->yawSpinBox->setValue(rcMapping[2]+1);
     }
 
     if (parameterName.contains("RC_MAP_THROTTLE")) {
-        rcMapping[3] = value.toInt();
-        ui->throttleSpinBox->setValue(rcMapping[3]);
+        rcMapping[3] = value.toInt() - 1;
+        ui->throttleSpinBox->setValue(rcMapping[3]+1);
     }
 
     if (parameterName.contains("RC_MAP_MODE_SW")) {
-        rcMapping[4] = value.toInt();
-        ui->modeSpinBox->setValue(rcMapping[4]);
+        rcMapping[4] = value.toInt() - 1;
+        ui->modeSpinBox->setValue(rcMapping[4]+1);
     }
 
     if (parameterName.contains("RC_MAP_AUX1")) {
-        rcMapping[5] = value.toInt();
-        ui->aux1SpinBox->setValue(rcMapping[5]);
+        rcMapping[5] = value.toInt() - 1;
+        ui->aux1SpinBox->setValue(rcMapping[5]+1);
     }
 
     if (parameterName.contains("RC_MAP_AUX2")) {
-        rcMapping[6] = value.toInt();
-        ui->aux1SpinBox->setValue(rcMapping[6]);
+        rcMapping[6] = value.toInt() - 1;
+        ui->aux1SpinBox->setValue(rcMapping[6]+1);
     }
 
     if (parameterName.contains("RC_MAP_AUX3")) {
-        rcMapping[7] = value.toInt();
-        ui->aux1SpinBox->setValue(rcMapping[7]);
+        rcMapping[7] = value.toInt() - 1;
+        ui->aux1SpinBox->setValue(rcMapping[7]+1);
     }
 
     // Scaling
 
     if (parameterName.contains("RC_SCALE_ROLL")) {
-        rcScaling[0] = value.toInt();
+        rcScaling[0] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_PITCH")) {
-        rcScaling[1] = value.toInt();
+        rcScaling[1] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_YAW")) {
-        rcScaling[2] = value.toInt();
+        rcScaling[2] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_THROTTLE")) {
-        rcScaling[3] = value.toInt();
+        rcScaling[3] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_MODE_SW")) {
-        rcScaling[4] = value.toInt();
+        rcScaling[4] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_AUX1")) {
-        rcScaling[5] = value.toInt();
+        rcScaling[5] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_AUX2")) {
-        rcScaling[6] = value.toInt();
+        rcScaling[6] = value.toFloat();
     }
 
     if (parameterName.contains("RC_SCALE_AUX3")) {
-        rcScaling[7] = value.toInt();
+        rcScaling[7] = value.toFloat();
     }
 }
 
@@ -575,45 +626,62 @@ void QGCVehicleConfig::updateView()
     {
         if (rc_mode == RC_MODE_1)
         {
-            ui->rollSlider->setValue(rcRoll);
-            ui->pitchSlider->setValue(rcThrottle);
-            ui->yawSlider->setValue(rcYaw);
-            ui->throttleSlider->setValue(rcPitch);
+            ui->rollSlider->setValue(rcRoll * 50 + 50);
+            ui->pitchSlider->setValue(rcThrottle * 100);
+            ui->yawSlider->setValue(rcYaw * 50 + 50);
+            ui->throttleSlider->setValue(rcPitch * 50 + 50);
         }
         else if (rc_mode == RC_MODE_2)
         {
-            ui->rollSlider->setValue(rcRoll);
-            ui->pitchSlider->setValue(rcPitch);
-            ui->yawSlider->setValue(rcYaw);
-            ui->throttleSlider->setValue(rcThrottle);
+            ui->rollSlider->setValue(rcRoll * 50 + 50);
+            ui->pitchSlider->setValue(rcPitch * 50 + 50);
+            ui->yawSlider->setValue(rcYaw * 50 + 50);
+            ui->throttleSlider->setValue(rcThrottle * 100);
         }
         else if (rc_mode == RC_MODE_3)
         {
-            ui->rollSlider->setValue(rcYaw);
-            ui->pitchSlider->setValue(rcThrottle);
-            ui->yawSlider->setValue(rcRoll);
-            ui->throttleSlider->setValue(rcPitch);
+            ui->rollSlider->setValue(rcYaw * 50 + 50);
+            ui->pitchSlider->setValue(rcThrottle * 100);
+            ui->yawSlider->setValue(rcRoll * 50 + 50);
+            ui->throttleSlider->setValue(rcPitch * 50 + 50);
         }
         else if (rc_mode == RC_MODE_4)
         {
-            ui->rollSlider->setValue(rcYaw);
-            ui->pitchSlider->setValue(rcPitch);
-            ui->yawSlider->setValue(rcRoll);
-            ui->throttleSlider->setValue(rcThrottle);
+            ui->rollSlider->setValue(rcYaw * 50 + 50);
+            ui->pitchSlider->setValue(rcPitch * 50 + 50);
+            ui->yawSlider->setValue(rcRoll * 50 + 50);
+            ui->throttleSlider->setValue(rcThrottle * 100);
+        }
+
+        ui->chanLabel->setText(QString("%1/%2").arg(rcValue[0]).arg(rcRoll, 5, 'f', 2, QChar(' ')));
+        ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[1]).arg(rcPitch, 5, 'f', 2, QChar(' ')));
+        ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[2]).arg(rcYaw, 5, 'f', 2, QChar(' ')));
+        ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[3]).arg(rcThrottle, 5, 'f', 2, QChar(' ')));
+
+        ui->modeSwitchSlider->setValue(rcMode * 50 + 50);
+        ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[4]).arg(rcMode, 5, 'f', 2, QChar(' ')));
+
+        if (rcValue[5] != UINT16_MAX) {
+            ui->aux1Slider->setValue(rcAux1 * 50 + 50);
+            ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[5]).arg(rcAux1, 5, 'f', 2, QChar(' ')));
+        } else {
+            ui->chanLabel_6->setText(tr("---"));
+        }
+
+        if (rcValue[6] != UINT16_MAX) {
+            ui->aux2Slider->setValue(rcAux2 * 50 + 50);
+            ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[6]).arg(rcAux2, 5, 'f', 2, QChar(' ')));
+        } else {
+            ui->chanLabel_7->setText(tr("---"));
+        }
+
+        if (rcValue[7] != UINT16_MAX) {
+            ui->aux3Slider->setValue(rcAux3 * 50 + 50);
+            ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[7]).arg(rcAux3, 5, 'f', 2, QChar(' ')));
+        } else {
+            ui->chanLabel_8->setText(tr("---"));
         }
 
-        ui->chanLabel->setText(QString("%1/%2").arg(rcValue[0]).arg(rcRoll));
-        ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[1]).arg(rcPitch));
-        ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[2]).arg(rcYaw));
-        ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[3]).arg(rcThrottle));
-        ui->modeSwitchSlider->setValue(rcMode);
-        ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[4]).arg(rcMode));
-        ui->aux1Slider->setValue(rcAux1);
-        ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[5]).arg(rcAux1));
-        ui->aux2Slider->setValue(rcAux2);
-        ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[6]).arg(rcAux2));
-        ui->aux3Slider->setValue(rcAux3);
-        ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[7]).arg(rcAux3));
         changed = false;
     }
 }
diff --git a/src/ui/QGCVehicleConfig.h b/src/ui/QGCVehicleConfig.h
index caec352a1..988f7f5ed 100644
--- a/src/ui/QGCVehicleConfig.h
+++ b/src/ui/QGCVehicleConfig.h
@@ -37,11 +37,81 @@ public slots:
     void stopCalibrationRC();
     /** Start/stop the RC calibration routine */
     void toggleCalibrationRC(bool enabled);
+    /** Set trim positions */
+    void setTrimPositions();
+    /** Detect which channels need to be inverted */
+    void detectChannelInversion();
     /** Change the mode setting of the control inputs */
     void setRCModeIndex(int newRcMode);
     /** Render the data updated */
     void updateView();
 
+    /** Set the RC channel */
+    void setRollChan(int channel) {
+        rcMapping[0] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setPitchChan(int channel) {
+        rcMapping[1] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setYawChan(int channel) {
+        rcMapping[2] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setThrottleChan(int channel) {
+        rcMapping[3] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setModeChan(int channel) {
+        rcMapping[4] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setAux1Chan(int channel) {
+        rcMapping[5] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setAux2Chan(int channel) {
+        rcMapping[6] = channel - 1;
+    }
+    /** Set the RC channel */
+    void setAux3Chan(int channel) {
+        rcMapping[7] = channel - 1;
+    }
+
+    /** Set channel inversion status */
+    void setRollInverted(bool inverted) {
+        rcRev[rcMapping[0]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setPitchInverted(bool inverted) {
+        rcRev[rcMapping[1]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setYawInverted(bool inverted) {
+        rcRev[rcMapping[2]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setThrottleInverted(bool inverted) {
+        rcRev[rcMapping[3]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setModeInverted(bool inverted) {
+        rcRev[rcMapping[4]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setAux1Inverted(bool inverted) {
+        rcRev[rcMapping[5]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setAux2Inverted(bool inverted) {
+        rcRev[rcMapping[6]] = inverted;
+    }
+    /** Set channel inversion status */
+    void setAux3Inverted(bool inverted) {
+        rcRev[rcMapping[7]] = inverted;
+    }
+
 protected slots:
     /** Reset the RC calibration */
     void resetCalibrationRC();
@@ -64,12 +134,13 @@ protected slots:
 protected:
     UASInterface* mav;                  ///< The current MAV
     static const unsigned int chanMax = 8;    ///< Maximum number of channels
+    unsigned int chanCount;               ///< Actual channels
     int rcType;                         ///< Type of the remote control
     quint64 rcTypeUpdateRequested;      ///< Zero if not requested, non-zero if requested
     static const unsigned int rcTypeTimeout = 5000; ///< 5 seconds timeout, in milliseconds
-    int rcMin[chanMax];                 ///< Minimum values
-    int rcMax[chanMax];                 ///< Maximum values
-    int rcTrim[chanMax];                ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle)
+    float rcMin[chanMax];                 ///< Minimum values
+    float rcMax[chanMax];                 ///< Maximum values
+    float rcTrim[chanMax];                ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle)
     int rcMapping[chanMax];             ///< PWM to function mappings
     float rcScaling[chanMax];           ///< Scaling of channel input to control commands
     bool rcRev[chanMax];                ///< Channel reverse
@@ -87,6 +158,7 @@ protected:
     QTimer updateTimer;                 ///< Controls update intervals
     enum RC_MODE rc_mode;               ///< Mode of the remote control, according to usual convention
     QList<QGCToolWidget*> toolWidgets;  ///< Configurable widgets
+    bool calibrationEnabled;            ///< calibration mode on / off
     
 private:
     Ui::QGCVehicleConfig *ui;
diff --git a/src/ui/QGCVehicleConfig.ui b/src/ui/QGCVehicleConfig.ui
index 89628d17c..2cda1d202 100644
--- a/src/ui/QGCVehicleConfig.ui
+++ b/src/ui/QGCVehicleConfig.ui
@@ -40,7 +40,7 @@
    <item row="0" column="0" colspan="2">
     <widget class="QTabWidget" name="tabWidget">
      <property name="currentIndex">
-      <number>1</number>
+      <number>0</number>
      </property>
      <widget class="QWidget" name="rcTab">
       <attribute name="title">
@@ -53,10 +53,10 @@
        <item row="8" column="1" rowspan="3">
         <widget class="QSlider" name="throttleSlider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="value">
           <number>0</number>
@@ -69,10 +69,10 @@
        <item row="8" column="4" rowspan="3" colspan="2">
         <widget class="QSlider" name="pitchSlider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="value">
           <number>0</number>
@@ -595,10 +595,10 @@
        <item row="11" column="0">
         <widget class="QSlider" name="yawSlider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="value">
           <number>0</number>
@@ -611,10 +611,10 @@
        <item row="11" column="3">
         <widget class="QSlider" name="rollSlider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="value">
           <number>0</number>
@@ -650,10 +650,10 @@
        <item row="8" column="10" rowspan="3">
         <widget class="QSlider" name="aux1Slider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="orientation">
           <enum>Qt::Vertical</enum>
@@ -663,10 +663,10 @@
        <item row="8" column="11" rowspan="3">
         <widget class="QSlider" name="aux2Slider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="orientation">
           <enum>Qt::Vertical</enum>
@@ -676,10 +676,10 @@
        <item row="8" column="12" rowspan="3" colspan="2">
         <widget class="QSlider" name="aux3Slider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="orientation">
           <enum>Qt::Vertical</enum>
@@ -689,10 +689,10 @@
        <item row="8" column="9" rowspan="3">
         <widget class="QSlider" name="modeSwitchSlider">
          <property name="minimum">
-          <number>-1</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <number>1</number>
+          <number>100</number>
          </property>
          <property name="orientation">
           <enum>Qt::Vertical</enum>
@@ -712,6 +712,13 @@
          </property>
         </spacer>
        </item>
+       <item row="11" column="14">
+        <widget class="QPushButton" name="setTrimButton">
+         <property name="text">
+          <string>Set Trim</string>
+         </property>
+        </widget>
+       </item>
       </layout>
      </widget>
      <widget class="QWidget" name="sensorTab">
@@ -815,8 +822,8 @@ p, li { white-space: pre-wrap; }
               <rect>
                <x>0</x>
                <y>0</y>
-               <width>651</width>
-               <height>203</height>
+               <width>98</width>
+               <height>28</height>
               </rect>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_4">
@@ -852,8 +859,8 @@ p, li { white-space: pre-wrap; }
               <rect>
                <x>0</x>
                <y>0</y>
-               <width>651</width>
-               <height>204</height>
+               <width>98</width>
+               <height>28</height>
               </rect>
              </property>
              <layout class="QHBoxLayout" name="horizontalLayout_4">
@@ -899,8 +906,8 @@ p, li { white-space: pre-wrap; }
               <rect>
                <x>0</x>
                <y>0</y>
-               <width>651</width>
-               <height>203</height>
+               <width>98</width>
+               <height>28</height>
               </rect>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_6">
@@ -959,8 +966,8 @@ p, li { white-space: pre-wrap; }
               <rect>
                <x>0</x>
                <y>0</y>
-               <width>651</width>
-               <height>204</height>
+               <width>98</width>
+               <height>28</height>
               </rect>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_7">
-- 
2.22.0