Merge branch 'skarlsso/OP-1474-PID_Scaling' into thread/OP-1474-PID_Scaling

Conflicts:
	shared/uavobjectdefinition/stabilizationsettings.xml

Makefile

@@ -638,7 +638,7 @@ uavo-collections_clean:
 #
 ##############################
 
-ALL_UNITTESTS := logfs
+ALL_UNITTESTS := logfs math
 
 # Build the directory for the unit tests
 UT_OUT_DIR := $(BUILD_DIR)/unit_tests

flight/libraries/math/mathmisc.h

@@ -31,6 +31,9 @@
 #ifndef MATHMISC_H
 #define MATHMISC_H
 
+#include <math.h>
+#include <stdint.h>
+
 // returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
 // returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
 // returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
@@ -79,4 +82,42 @@ static inline void vector_normalizef(float *vector, const uint8_t dim)
     }
 }
 
+typedef struct pointf {
+    float x;
+    float y;
+} pointf;
+
+// Returns the y value, given x, on the line passing through the points p0 and p1.
+static inline float y_on_line(float x, pointf *p0, pointf *p1)
+{
+    // Setup line y = m * x + b.
+    const float dY1 = p1->y - p0->y;
+    const float dX1 = p1->x - p0->x;
+    const float m   = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==>
+    const float b   = p0->y - m * p0->x;
+
+    // Get the y value on the line.
+    return m * x + b;
+}
+
+// Returns the y value, given x, on the curve defined by the points array.
+// The fist and last line of the curve extends beyond the first resp. last points.
+static inline float y_on_curve(float x, pointf points[], int num_points)
+{
+    // Find the two points x is within.
+    // If x is smaller than the first point's x value, use the first line of the curve.
+    // If x is larger than the last point's x value, user the last line of the curve.
+    int end_point = num_points - 1;
+
+    for (int i = 1; i < num_points; i++) {
+        if (x < points[i].x) {
+            end_point = i;
+            break;
+        }
+    }
+
+    // Find the y value on the selected line.
+    return y_on_line(x, &points[end_point - 1], &points[end_point]);
+}
+
 #endif /* MATHMISC_H */

flight/libraries/math/pid.c

@@ -140,3 +140,32 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
     pid->d    = d;
     pid->iLim = iLim;
 }
+
+float pid_scale_factor(pid_scaler *scaler)
+{
+    float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
+
+    return 1.0f + (IS_REAL(y) ? y : 0.0f);
+}
+
+float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT,
+                                pid_scaler *scaler)
+{
+    // Save PD values.
+    float p     = pid->p;
+    float d     = pid->d;
+
+    // Scale PD values.
+    float scale = pid_scale_factor(scaler);
+
+    pid->p *= scale;
+    pid->d *= scale;
+
+    float value = pid_apply_setpoint(pid, factor, setpoint, measured, dT);
+
+    // Restore PD values.
+    pid->p = p;
+    pid->d = d;
+
+    return value;
+}

flight/libraries/math/pid.h

@@ -31,6 +31,8 @@
 #ifndef PID_H
 #define PID_H
 
+#include "mathmisc.h"
+
 // !
 struct pid {
     float p;
@@ -42,11 +44,19 @@ struct pid {
     float lastDer;
 };
 
+typedef struct pid_scaler {
+    float  x;
+    pointf points[5];
+} pid_scaler;
+
 // ! Methods to use the pid structures
 float pid_apply(struct pid *pid, const float err, float dT);
 float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
+float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT,
+                                pid_scaler *scaler);
 void pid_zero(struct pid *pid);
 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
 void pid_configure_derivative(float cutoff, float gamma);
+float pid_scale_factor(pid_scaler *scaler);
 
 #endif /* PID_H */

flight/modules/Stabilization/innerloop.c

@@ -42,6 +42,8 @@
 #include <flightstatus.h>
 #include <manualcontrolcommand.h>
 #include <stabilizationbank.h>
+#include <stabilizationdesired.h>
+#include <actuatordesired.h>
 
 #include <stabilization.h>
 #include <relay_tuning.h>
@@ -80,6 +82,8 @@ void stabilizationInnerloopInit()
     StabilizationStatusInitialize();
     FlightStatusInitialize();
     ManualControlCommandInitialize();
+    StabilizationDesiredInitialize();
+    ActuatorDesiredInitialize();
 #ifdef REVOLUTION
     AirspeedStateInitialize();
     AirspeedStateConnectCallback(AirSpeedUpdatedCb);
@@ -93,6 +97,58 @@ void stabilizationInnerloopInit()
     PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
 }
 
+static float get_pid_scale_source_value()
+{
+    float value;
+
+    switch (stabSettings.settings.ThrustPIDScaleSource) {
+    case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE:
+        ManualControlCommandThrottleGet(&value);
+        break;
+    case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST:
+        StabilizationDesiredThrustGet(&value);
+        break;
+    case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIRETHRUST:
+        ActuatorDesiredThrustGet(&value);
+        break;
+    default:
+        ManualControlCommandThrottleGet(&value);
+        break;
+    }
+
+    if (value < 0) {
+        value = 0.0f;
+    }
+
+    return value;
+}
+
+static inline pid_scaler create_pid_scaler()
+{
+    float throttle;
+
+    ManualControlCommandThrottleGet(&throttle);
+
+    struct pid_scaler scaler = {
+        .x      = get_pid_scale_source_value(),
+        .points = {
+            { 0.0f,  stabSettings.settings.ThrustPIDScaleCurve[0] },
+            { 0.25f, stabSettings.settings.ThrustPIDScaleCurve[1] },
+            { 0.50f, stabSettings.settings.ThrustPIDScaleCurve[2] },
+            { 0.75f, stabSettings.settings.ThrustPIDScaleCurve[3] },
+            { 1.0f,  stabSettings.settings.ThrustPIDScaleCurve[4] }
+        }
+    };
+
+    return scaler;
+}
+
+static int is_pid_scaled_for_axis(int axis)
+{
+    return stabSettings.settings.EnableThrustPIDScaling
+           && (axis == 0 // Roll
+               || axis == 1); // Pitch
+}
 
 /**
  * WARNING! This callback executes with critical flight control priority every
@@ -200,7 +256,12 @@ static void stabilizationInnerloopTask()
                                  -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
                                  StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
                                  );
-                actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
+                if (is_pid_scaled_for_axis(t)) {
+                    pid_scaler scaler = create_pid_scaler();
+                    actuatorDesiredAxis[t] = pid_apply_setpoint_scaled(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT, &scaler);
+                } else {
+                    actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
+                }
                 break;
             case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
             default:

flight/pios/common/pios_sbus.c

@@ -250,7 +250,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
         state->received_data[state->byte_count - 1] = b;
         state->byte_count++;
     } else {
-        if (b == SBUS_EOF_BYTE || (b % SBUS_R7008SB_EOF_COUNTER_MASK) == SBUS_R7008SB_EOF_BYTE) {
+        if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) {
             /* full frame received */
             uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
             if (flags & SBUS_FLAG_FL) {

flight/pios/inc/pios_sbus_priv.h

@@ -66,8 +66,7 @@
 #define SBUS_FLAG_FL                  0x04
 #define SBUS_FLAG_FS                  0x08
 
-#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCF
-#define SBUS_R7008SB_EOF_BYTE         0x04
+#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCB
 
 /*
  * S.Bus protocol provides 16 proportional and 2 discrete channels.

flight/tests/math/Makefile

@@ -0,0 +1,37 @@
+###############################################################################
+# @file       Makefile
+# @author     PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
+#             Copyright (c) 2013, The OpenPilot Team, http://www.openpilot.org
+# @addtogroup 
+# @{
+# @addtogroup 
+# @{
+# @brief Makefile for unit test
+###############################################################################
+#
+# 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, write to the Free Software Foundation, Inc.,
+# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+#
+
+ifndef OPENPILOT_IS_COOL
+    $(error Top level Makefile must be used to build this target)
+endif
+
+include $(ROOT_DIR)/make/firmware-defs.mk
+
+EXTRAINCDIRS += $(TOPDIR)
+EXTRAINCDIRS += $(ROOT_DIR)/flight/libraries/math
+EXTRAINCDIRS += $(PIOS)/inc
+
+include $(ROOT_DIR)/make/unittest.mk

flight/tests/math/unittest.cpp

@@ -0,0 +1,110 @@
+#include "gtest/gtest.h"
+
+#include <stdio.h> /* printf */
+#include <stdlib.h> /* abort */
+#include <string.h> /* memset */
+
+extern "C" {
+#include "mathmisc.h"
+}
+
+#define epsilon 0.00001f
+// From pios_math.h
+#define IS_REAL(f)           (!isnan(f) && !isinf(f))
+#define length(points_array) (sizeof(points_array) / sizeof(points_array[0]))
+
+// To use a test fixture, derive a class from testing::Test.
+class MathTestRaw : public testing::Test {};
+
+TEST_F(MathTestRaw, y_on_line0) {
+    pointf points[] = {
+        { 0.0f, -0.30f },
+        { 0.5f, 0.30   }
+    };
+
+    EXPECT_NEAR(-0.60f, y_on_line(-0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(-0.30f, y_on_line(0.00f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.00f, y_on_line(0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.60f, y_on_line(0.75f, &points[0], &points[1]), epsilon);
+}
+
+TEST_F(MathTestRaw, y_on_line1) {
+    pointf points[] = {
+        { 0.25f, -0.30f },
+        { 0.50f, 0.30   }
+    };
+
+    EXPECT_NEAR(-1.50f, y_on_line(-0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(-0.90f, y_on_line(0.00f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(-0.30f, y_on_line(0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.90f, y_on_line(0.75f, &points[0], &points[1]), epsilon);
+}
+
+TEST_F(MathTestRaw, y_on_line2) {
+    pointf points[] = {
+        { -0.25f, -0.30f },
+        { 0.50f,  0.30   }
+    };
+
+    EXPECT_NEAR(-0.30f, y_on_line(-0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(-0.10f, y_on_line(0.00f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.10f, y_on_line(0.25f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon);
+    EXPECT_NEAR(0.50f, y_on_line(0.75f, &points[0], &points[1]), epsilon);
+}
+
+TEST_F(MathTestRaw, y_on_line3) {
+    pointf points[] = {
+        { 0.25f, -0.30f },
+        { 0.25f, 0.30   }
+    };
+
+
+    EXPECT_FALSE(IS_REAL(y_on_line(-0.25f, &points[0], &points[1])));
+}
+
+TEST_F(MathTestRaw, y_on_curve0) {
+    pointf points[] = {
+        { 0.00f, -0.40f },
+        { 0.25f, -0.20f },
+        { 0.50f, 0.00f  },
+        { 0.75f, 0.20   },
+        { 1.00f, 0.40   }
+    };
+
+    EXPECT_NEAR(-0.50f, y_on_curve(-0.125f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.40f, y_on_curve(0.000f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.30f, y_on_curve(0.125f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.20f, y_on_curve(0.250f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.10f, y_on_curve(0.375f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.00f, y_on_curve(0.500f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.10f, y_on_curve(0.625f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.20f, y_on_curve(0.750f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.30f, y_on_curve(0.875f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.40f, y_on_curve(1.000f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.50f, y_on_curve(1.125f, points, length(points)), epsilon);
+}
+
+
+TEST_F(MathTestRaw, y_on_curve1) {
+    pointf points[] = {
+        { -0.25f, 0.10f },
+        { 0.00f,  0.20f },
+        { 0.50f,  0.30f },
+        { 1.00f,  -0.30 },
+        { 2.00f,  -0.50 }
+    };
+
+    EXPECT_NEAR(0.00f, y_on_curve(-0.500f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.10f, y_on_curve(-0.250f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.15f, y_on_curve(-0.125f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.20f, y_on_curve(0.000f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.22f, y_on_curve(0.100f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.30f, y_on_curve(0.500f, points, length(points)), epsilon);
+    EXPECT_NEAR(0.00f, y_on_curve(0.750f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.30f, y_on_curve(1.000f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.35f, y_on_curve(1.250f, points, length(points)), epsilon);
+    EXPECT_NEAR(-0.50f, y_on_curve(2.000f, points, length(points)), epsilon);
+}

ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml

@@ -1779,7 +1779,7 @@
           <showTileGridLines>false</showTileGridLines>
           <uavSymbol>mapquad.png</uavSymbol>
           <useMemoryCache>true</useMemoryCache>
-          <useOpenGL>true</useOpenGL>
+          <useOpenGL>false</useOpenGL>
         </data>
       </Google__PCT__20Sat>
       <Memory__PCT__20Only>
@@ -1799,7 +1799,7 @@
           <showTileGridLines>false</showTileGridLines>
           <uavSymbol>airplanepip.png</uavSymbol>
           <useMemoryCache>true</useMemoryCache>
-          <useOpenGL>true</useOpenGL>
+          <useOpenGL>false</useOpenGL>
         </data>
       </Memory__PCT__20Only>
       <default>
@@ -1819,7 +1819,7 @@
           <showTileGridLines>false</showTileGridLines>
           <uavSymbol>mapquad.png</uavSymbol>
           <useMemoryCache>true</useMemoryCache>
-          <useOpenGL>true</useOpenGL>
+          <useOpenGL>false</useOpenGL>
         </data>
       </default>
     </OPMapGadget>    

ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp

@@ -126,6 +126,17 @@ void ConfigOutputWidget::enableControls(bool enable)
     ui->channelOutTest->setEnabled(enable);
 }
 
+/**
+   Force update all channels with the values in the OutputChannelForms.
+ */
+void ConfigOutputWidget::sendAllChannelTests()
+{
+    for (unsigned int i = 0; i < ActuatorCommand::CHANNEL_NUMELEM; i++) {
+        OutputChannelForm *form = getOutputChannelForm(i);
+        sendChannelTest(i, form->neutral());
+    }
+}
+
 /**
    Toggles the channel testing mode by making the GCS take over
    the ActuatorCommand objects
@@ -178,6 +189,11 @@ void ConfigOutputWidget::runChannelTests(bool state)
     }
     obj->setMetadata(mdata);
     obj->updated();
+
+    // Setup the correct initial channel values when the channel testing mode is turned on.
+    if (state) {
+        sendAllChannelTests();
+    }
 }
 
 OutputChannelForm *ConfigOutputWidget::getOutputChannelForm(const int index) const

ground/openpilotgcs/src/plugins/config/configoutputwidget.h

@@ -58,7 +58,11 @@ private:
     void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value);
 
     void assignOutputChannel(UAVDataObject *obj, QString &str);
+
     OutputChannelForm *getOutputChannelForm(const int index) const;
+
+    void sendAllChannelTests();
+
     int mccDataRate;
 
     UAVObject::Metadata accInitialData;

ground/openpilotgcs/src/plugins/config/inputchannelform.ui

@@ -120,6 +120,9 @@ margin:1px;</string>
      <property name="buttonSymbols">
       <enum>QAbstractSpinBox::UpDownArrows</enum>
      </property>
+     <property name="keyboardTracking">
+      <bool>false</bool>
+     </property>
      <property name="maximum">
       <number>9999</number>
      </property>
@@ -670,6 +673,9 @@ margin:1px;</string>
      <property name="buttonSymbols">
       <enum>QAbstractSpinBox::UpDownArrows</enum>
      </property>
+     <property name="keyboardTracking">
+      <bool>false</bool>
+     </property>
      <property name="maximum">
       <number>9999</number>
      </property>
@@ -762,6 +768,9 @@ margin:1px;</string>
         <property name="alignment">
          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
         </property>
+        <property name="keyboardTracking">
+         <bool>false</bool>
+        </property>
         <property name="maximum">
          <number>9999</number>
         </property>

ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp

@@ -76,8 +76,10 @@ QIODevice *LoggingConnection::openDevice(const QString &deviceName)
     QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open file"), QString(""), tr("OpenPilot Log (*.opl)"));
     if (!fileName.isNull()) {
         startReplay(fileName);
+        return &logFile;
     }
-    return &logFile;
+
+    return NULL;
 }
 
 void LoggingConnection::startReplay(QString file)

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -3,56 +3,57 @@
     <object name="StabilizationSettings" singleinstance="true" settings="true" category="Control">
         <description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description>
 
-    <!-- Note: The number of elements here must match the number of available flight mode switch positions -->
-    <field name="FlightModeMap" units="" type="enum"
-        options="Bank1,Bank2,Bank3"
-        elements="6"
-        defaultvalue="Bank1,Bank1,Bank1,Bank1,Bank1,Bank1"/>
+	<!-- Note: The number of elements here must match the number of available flight mode switch positions -->
+	<field name="FlightModeMap" units="" type="enum"
+	    options="Bank1,Bank2,Bank3"
+	    elements="6"
+	    defaultvalue="Bank1,Bank1,Bank1,Bank1,Bank1,Bank1"/>
 
-    <field name="VbarSensitivity" units="frac" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0.5,0.5,0.5"/>
-    <field name="VbarRollPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
-    <field name="VbarPitchPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
-    <field name="VbarYawPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
-    <field name="VbarTau" units="sec" type="float" elements="1" defaultvalue="0.5"/>
-    <field name="VbarGyroSuppress" units="%" type="int8" elements="1" defaultvalue="30"/>
-    <field name="VbarPiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
-    <field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
+	<field name="VbarSensitivity" units="frac" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0.5,0.5,0.5"/>
+	<field name="VbarRollPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
+	<field name="VbarPitchPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
+	<field name="VbarYawPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
+	<field name="VbarTau" units="sec" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="VbarGyroSuppress" units="%" type="int8" elements="1" defaultvalue="30"/>
+	<field name="VbarPiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
+	<field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
 
-    <field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
-    <field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
-    <field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
+	<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
+	<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
+	<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
 
-    <field name="AxisLockKp" units="" type="float" elements="1" defaultvalue="2.5"/>
-    <field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
-    <field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
+	<field name="AxisLockKp" units="" type="float" elements="1" defaultvalue="2.5"/>
+	<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
+	<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
 
-    <field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
-    <field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
+	<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
+	<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
 
-    <field name="RattitudeModeTransition" units="%" type="uint8" elements="1" defaultvalue="80"/>
+	<field name="RattitudeModeTransition" units="%" type="uint8" elements="1" defaultvalue="80"/>
 
-    <field name="CruiseControlMinThrust"           units="%"   type="int8"  elements="1" defaultvalue="5"/>
-    <field name="CruiseControlMaxThrust"           units="%"   type="uint8" elements="1" defaultvalue="90"/>
-    <field name="CruiseControlMaxAngle"            units="deg" type="uint8" elements="1" defaultvalue="105"/>
-    <field name="CruiseControlMaxPowerFactor"      units="x"   type="float" elements="1" defaultvalue="3.0"/>
-    <field name="CruiseControlPowerTrim"           units="%"   type="float" elements="1" defaultvalue="100.0"/>
-    <field name="CruiseControlPowerDelayComp"      units="sec" type="float" elements="1" defaultvalue="0.25"/>
-    <field name="CruiseControlFlightModeSwitchPosEnable" units="" type="enum" elements="6" options="FALSE,TRUE" defaultvalue="FALSE"/>
+	<field name="CruiseControlMinThrust"           units="%"   type="int8"  elements="1" defaultvalue="5"/>
+	<field name="CruiseControlMaxThrust"           units="%"   type="uint8" elements="1" defaultvalue="90"/>
+	<field name="CruiseControlMaxAngle"            units="deg" type="uint8" elements="1" defaultvalue="105"/>
+	<field name="CruiseControlMaxPowerFactor"      units="x"   type="float" elements="1" defaultvalue="3.0"/>
+	<field name="CruiseControlPowerTrim"           units="%"   type="float" elements="1" defaultvalue="100.0"/>
+	<field name="CruiseControlPowerDelayComp"      units="sec" type="float" elements="1" defaultvalue="0.25"/>
+	<field name="CruiseControlFlightModeSwitchPosEnable" units="" type="enum" elements="6" options="FALSE,TRUE" defaultvalue="FALSE"/>
 
-    <field name="CruiseControlInvertedThrustReversing" units="" type="enum" elements="1" options="Unreversed,Reversed" defaultvalue="Unreversed"/>
-    <field name="CruiseControlInvertedPowerOutput"     units="" type="enum" elements="1" options="Zero,Normal,Boosted" defaultvalue="Zero"/>
+	<field name="CruiseControlInvertedThrustReversing" units="" type="enum" elements="1" options="Unreversed,Reversed" defaultvalue="Unreversed"/>
+	<field name="CruiseControlInvertedPowerOutput"     units="" type="enum" elements="1" options="Zero,Normal,Boosted" defaultvalue="Zero"/>
 
-    <field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
+	<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
 
-    <field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
-    <field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
+	<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
+	<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
 
-    <field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
-    <field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
+	<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
+	<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
+	<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesireThrust" defaultvalue="ManualControlThrottle" />
 
-    <access gcs="readwrite" flight="readwrite"/>
-    <telemetrygcs acked="true" updatemode="onchange" period="0"/>
-    <telemetryflight acked="true" updatemode="onchange" period="0"/>
-    <logging updatemode="manual" period="0"/>
+	<access gcs="readwrite" flight="readwrite"/>
+	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
+	<telemetryflight acked="true" updatemode="onchange" period="0"/>
+	<logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/stabilizationsettingsbank1.xml

@@ -2,15 +2,15 @@
     <object name="StabilizationSettingsBank1" singleinstance="true" settings="true" category="Control">
         <description>Currently selected PID bank</description>
 
-	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
+	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
-	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0050,0.0050,0,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
+	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.00620,0.01000,0.00005,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
 	<field name="RollPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>

shared/uavobjectdefinition/stabilizationsettingsbank2.xml

@@ -2,15 +2,15 @@
     <object name="StabilizationSettingsBank2" singleinstance="true" settings="true" category="Control">
         <description>Currently selected PID bank</description>
 
-	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
+	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
-	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0050,0.0050,0,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
+	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.00620,0.01000,0.00005,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
 	<field name="RollPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>

shared/uavobjectdefinition/stabilizationsettingsbank3.xml

@@ -2,15 +2,15 @@
     <object name="StabilizationSettingsBank3" singleinstance="true" settings="true" category="Control">
         <description>Currently selected PID bank</description>
 
-	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
-	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="55" limits="%BE:0:180"/>
+	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
+	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 <field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
-	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0050,0.0050,0,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
+	<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.00620,0.01000,0.00005,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
 	<field name="RollPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>