OP-1605 Normalize Mixer values for all frames. Fix aileron mixing (wizard). Fix Vtail aileron mixing + Vtail mixing (config plugin)

Mixing values for sliders are now saved for Fixed Wing familly.

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp

@@ -112,6 +112,13 @@ void ConfigFixedWingWidget::setupUI(QString frameType)
     planeimg = new QGraphicsSvgItem();
     planeimg->setSharedRenderer(renderer);
 
+    UAVDataObject *system = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("SystemSettings")));
+    Q_ASSERT(system);
+    QPointer<UAVObjectField> field = system->getField(QString("AirframeType"));
+
+    UAVDataObject *mixer = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("MixerSettings")));
+    Q_ASSERT(mixer);
+
     if (frameType == "FixedWing" || frameType == "Aileron") {
         planeimg->setElementId("aileron");
         setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Aileron"));
@@ -129,6 +136,9 @@ void ConfigFixedWingWidget::setupUI(QString frameType)
 
         m_aircraft->elevonSlider1->setEnabled(false);
         m_aircraft->elevonSlider2->setEnabled(false);
+
+        m_aircraft->elevonSlider1->setValue(100);
+        m_aircraft->elevonSlider2->setValue(100);
     } else if (frameType == "FixedWingElevon" || frameType == "Elevon") {
         planeimg->setElementId("elevon");
         setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon"));
@@ -147,6 +157,15 @@ void ConfigFixedWingWidget::setupUI(QString frameType)
 
         m_aircraft->elevonSlider1->setEnabled(true);
         m_aircraft->elevonSlider2->setEnabled(true);
+
+        // Get values saved if frameType = current frameType set on board
+        if (field->getValue().toString() == "FixedWingElevon") {
+            m_aircraft->elevonSlider1->setValue(getMixerValue(mixer, "MixerValueRoll"));
+            m_aircraft->elevonSlider2->setValue(getMixerValue(mixer, "MixerValuePitch"));
+        } else {
+            m_aircraft->elevonSlider1->setValue(100);
+            m_aircraft->elevonSlider2->setValue(100);
+        }
     } else if (frameType == "FixedWingVtail" || frameType == "Vtail") {
         planeimg->setElementId("vtail");
         setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail"));
@@ -168,7 +187,17 @@ void ConfigFixedWingWidget::setupUI(QString frameType)
 
         m_aircraft->elevonSlider1->setEnabled(true);
         m_aircraft->elevonSlider2->setEnabled(true);
+
+        // Get values saved if frameType = current frameType set on board
+        if (field->getValue().toString() == "FixedWingVtail") {
+            m_aircraft->elevonSlider1->setValue(getMixerValue(mixer, "MixerValueYaw"));
+            m_aircraft->elevonSlider2->setValue(getMixerValue(mixer, "MixerValuePitch"));
+        } else {
+            m_aircraft->elevonSlider1->setValue(100);
+            m_aircraft->elevonSlider2->setValue(100);
+        }
     }
+
     QGraphicsScene *scene = new QGraphicsScene();
     scene->addItem(planeimg);
     scene->setSceneRect(planeimg->boundingRect());
@@ -240,26 +269,13 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType)
     setComboCurrentIndex(m_aircraft->fwRudder1ChannelBox, fixed.FixedWingYaw1);
     setComboCurrentIndex(m_aircraft->fwRudder2ChannelBox, fixed.FixedWingYaw2);
 
-    if (frameType == "FixedWingElevon") {
+    // Get mixing values for GUI sliders (values stored onboard)
-        // If the airframe is elevon, restore the slider setting
+    if (frameType == "FixedWingElevon" || frameType == "Elevon") {
-        // Find the channel number for Elevon1 (FixedWingRoll1)
+        m_aircraft->elevonSlider1->setValue(getMixerValue(mixer, "MixerValueRoll"));
-        int channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1;
+        m_aircraft->elevonSlider2->setValue(getMixerValue(mixer, "MixerValuePitch"));
-        if (channel > -1) {
+    } else if (frameType == "FixedWingVtail" || frameType == "Vtail") {
-            // If for some reason the actuators were incoherent, we might fail here, hence the check.
+        m_aircraft->elevonSlider1->setValue(getMixerValue(mixer, "MixerValueYaw"));
-            m_aircraft->elevonSlider1->setValue(
+        m_aircraft->elevonSlider2->setValue(getMixerValue(mixer, "MixerValuePitch"));
-                getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) / 1.27);
-            m_aircraft->elevonSlider2->setValue(
-                -getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) / 1.27);
-        }
-    } else if (frameType == "FixedWingVtail") {
-        int channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1;
-        if (channel > -1) {
-            // If for some reason the actuators were incoherent, we might fail here, hence the check.
-            m_aircraft->elevonSlider1->setValue(
-                getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) / 1.27);
-            m_aircraft->elevonSlider2->setValue(
-                getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) / 1.27);
-        }
     }
 }
 
@@ -404,37 +420,48 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType)
 
     // 1. Assign the servo/motor/none for each channel
 
-    double value;
+    double pitch;
+    double roll;
+    double yaw;
 
     // motor
     int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1;
     setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR);
     setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
 
-    // rudders
+    // Yaw mixer value, currently no slider (should be added for Rudder response ?)
+    yaw     = 127;
+    setMixerValue(mixer, "MixerValueYaw", 100);
+
     channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1;
     setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127);
+    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, yaw);
 
     channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1;
     setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127);
+    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -yaw);
 
     // ailerons
     channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1;
     if (channel > -1) {
-        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        // Compute mixer absolute values
-        value   = (double)(m_aircraft->elevonSlider2->value() * 1.27);
+        pitch = (double)(m_aircraft->elevonSlider2->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, -value);
+        roll  = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        value   = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value);
 
+        // Store sliders values onboard
+        setMixerValue(mixer, "MixerValuePitch", m_aircraft->elevonSlider2->value());
+        setMixerValue(mixer, "MixerValueRoll", m_aircraft->elevonSlider1->value());
+
+        // First servo (left aileron)
+        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, -pitch);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll);
+
+        // Second servo (right aileron)
         channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1;
         setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-        value   = (double)(m_aircraft->elevonSlider2->value() * 1.27);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, pitch);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll);
-        value   = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value);
     }
 
     m_aircraft->fwStatusLabel->setText("Mixer generated");
@@ -474,46 +501,54 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType)
 
     // 1. Assign the servo/motor/none for each channel
 
+    double pitch;
+    double roll;
+    double yaw;
+
     // motor
     int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1;
     setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR);
     setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
 
-    // rudders
-    channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1;
-    setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127);
-
-    channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1;
-    setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127);
-
     // ailerons
     channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1;
     if (channel > -1) {
-        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        // Roll mixer value, currently no slider (should be added for Ailerons response ?)
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, 127);
+        roll = 127;
+        // Store Roll fixed value onboard
+        setMixerValue(mixer, "MixerValueRoll", 100);
 
+        // First Aileron (left)
+        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll);
+
+        // Second Aileron (right)
         channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1;
         setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -127);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll);
     }
 
-    // vtail
+    // vtail (pitch / yaw mixing)
     channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1;
     if (channel > -1) {
-        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        // Compute mixer absolute values
-        double value = (double)(m_aircraft->elevonSlider2->value() * 1.27);
+        pitch = (double)(m_aircraft->elevonSlider2->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value);
+        yaw   = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        value   = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, value);
 
+        // Store sliders values onboard
+        setMixerValue(mixer, "MixerValuePitch", m_aircraft->elevonSlider2->value());
+        setMixerValue(mixer, "MixerValueYaw", m_aircraft->elevonSlider1->value());
+
+        // First Vtail servo
+        setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, -pitch);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, yaw);
+
+        // Second Vtail servo
         channel = m_aircraft->fwElevator2ChannelBox->currentIndex() - 1;
         setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
-        value   = (double)(m_aircraft->elevonSlider2->value() * 1.27);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, pitch);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value);
+        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, yaw);
-        value   = (double)(m_aircraft->elevonSlider1->value() * 1.27);
-        setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -value);
     }
 
     m_aircraft->fwStatusLabel->setText("Mixer generated");

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp

@@ -807,18 +807,13 @@ void ConfigMultiRotorWidget::setupQuadMotor(int channel, double pitch, double ro
 
     Q_ASSERT(mixer);
 
-    // Normalize mixer values, allow a well balanced mixer saved
-    pitch = (pitch < 0) ? qFloor(pitch * 127) : qCeil(pitch * 127);
-    roll  = (roll < 0) ? qFloor(roll * 127) : qCeil(roll * 127);
-    yaw   = (yaw < 0) ? qFloor(yaw * 127) : qCeil(yaw * 127);
-
     setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR);
 
     setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
     setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 0);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll);
+    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll * 127);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, pitch);
+    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, pitch * 127);
-    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, yaw);
+    setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, yaw * 127);
 }
 
 /**

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp

@@ -29,6 +29,7 @@
 #include "uavobjectmanager.h"
 #include "uavobject.h"
 #include "systemsettings.h"
+#include <QtCore/qmath.h>
 
 #include <QDebug>
 
@@ -254,6 +255,9 @@ void VehicleConfig::setMixerVectorValue(UAVDataObject *mixer, int channel, Mixer
         UAVObjectField *field = mixer->getField(mixerVectors.at(channel));
         Q_ASSERT(field);
 
+        // Normalize mixer values
+        value = (value < 0) ? qFloor(value) : qCeil(value);
+
         if (field) {
             field->setDouble(value, elementName);
         }

ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp

@@ -690,6 +690,10 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch
         break;
     }
     case VehicleConfigurationSource::VEHICLE_FIXEDWING:
+        mSettings->setMixerValueRoll(100);
+        mSettings->setMixerValuePitch(100);
+        mSettings->setMixerValueYaw(100);
+        break;
     case VehicleConfigurationSource::VEHICLE_HELI:
     case VehicleConfigurationSource::VEHICLE_SURFACE:
         // TODO: Implement mixer / sliders settings for other vehicle types?
@@ -1828,7 +1832,7 @@ void VehicleConfigurationHelper::setupDualAileron()
     channels[0].type      = MIXER_TYPE_SERVO;
     channels[0].throttle1 = 0;
     channels[0].throttle2 = 0;
-    channels[0].roll      = -100;
+    channels[0].roll      = 100;
     channels[0].pitch     = 0;
     channels[0].yaw       = 0;