Merge remote-tracking branch 'origin/skarlsso/OP-1397_add_responsiveness_to_txpid' into next

flight/modules/TxPID/txpid.c

@@ -83,6 +83,7 @@
 // Private functions
 static void updatePIDs(UAVObjEvent *ev);
 static uint8_t update(float *var, float val);
+static uint8_t updateUint8(uint8_t *var, float val);
 static float scale(float val, float inMin, float inMax, float outMin, float outMax);
 
 /**
@@ -226,6 +227,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_ROLLRATERESP:
+                needsUpdateBank |= update(&bank.ManualRate.Roll, value);
+                break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
                 break;
@@ -235,6 +239,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
+                needsUpdateBank |= updateUint8(&bank.RollMax, value);
+                break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKP:
                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                 break;
@@ -247,6 +254,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_PITCHRATERESP:
+                needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
+                break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                 break;
@@ -256,6 +266,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
+                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
+                break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
                 needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
@@ -272,6 +285,10 @@ static void updatePIDs(UAVObjEvent *ev)
                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
+                needsUpdateBank |= update(&bank.ManualRate.Roll, value);
+                needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
+                break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
@@ -284,6 +301,10 @@ static void updatePIDs(UAVObjEvent *ev)
                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
+                needsUpdateBank |= updateUint8(&bank.RollMax, value);
+                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
+                break;
             case TXPIDSETTINGS_PIDS_YAWRATEKP:
                 needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
                 break;
@@ -296,6 +317,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
                 needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_YAWRATERESP:
+                needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
+                break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
                 needsUpdateBank |= update(&bank.YawPI.Kp, value);
                 break;
@@ -305,6 +329,9 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
                 needsUpdateBank |= update(&bank.YawPI.ILimit, value);
                 break;
+            case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
+                needsUpdateBank |= updateUint8(&bank.YawMax, value);
+                break;
             case TXPIDSETTINGS_PIDS_GYROTAU:
                 needsUpdateStab |= update(&stab.GyroTau, value);
                 break;
@@ -389,6 +416,21 @@ static uint8_t update(float *var, float val)
     return 0;
 }
 
+/**
+ * Updates var using val if needed.
+ * \returns 1 if updated, 0 otherwise
+ */
+static uint8_t updateUint8(uint8_t *var, float val)
+{
+    uint8_t roundedVal = (uint8_t)roundf(val);
+
+    if (*var != roundedVal) {
+        *var = roundedVal;
+        return 1;
+    }
+    return 0;
+}
+
 /**
  * @}
  */

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

@@ -61,14 +61,15 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
 
     addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true);
 
-    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
-    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
-    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
-
     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1);
     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2);
     addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3);
 
+    // It's important that the PIDx values are populated before the MinPIDx and MaxPIDx,
+    // otherwise the MinPIDx and MaxPIDx will be capped by the old spin box limits. The correct limits
+    // are set when updateSpinBoxProperties is called when the PIDx->currentTextChanged signal is sent.
+    // The binding order is reversed because the values are populated in reverse.
+
     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1);
     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2);
     addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3);
@@ -77,6 +78,10 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
     addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2);
     addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3);
 
+    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
+    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
+    addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
+
     addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN);
     addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX);
 
@@ -96,6 +101,50 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget()
     // Do nothing
 }
 
+static bool isResponsivenessOption(int pidOption)
+{
+    switch (pidOption) {
+    case TxPIDSettings::PIDS_ROLLRATERESP:
+    case TxPIDSettings::PIDS_PITCHRATERESP:
+    case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
+    case TxPIDSettings::PIDS_YAWRATERESP:
+    case TxPIDSettings::PIDS_ROLLATTITUDERESP:
+    case TxPIDSettings::PIDS_PITCHATTITUDERESP:
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
+    case TxPIDSettings::PIDS_YAWATTITUDERESP:
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+static bool isAttitudeOption(int pidOption)
+{
+    switch (pidOption) {
+    case TxPIDSettings::PIDS_ROLLATTITUDEKP:
+    case TxPIDSettings::PIDS_PITCHATTITUDEKP:
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP:
+    case TxPIDSettings::PIDS_YAWATTITUDEKP:
+    case TxPIDSettings::PIDS_ROLLATTITUDEKI:
+    case TxPIDSettings::PIDS_PITCHATTITUDEKI:
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI:
+    case TxPIDSettings::PIDS_YAWATTITUDEKI:
+    case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT:
+    case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT:
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT:
+    case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
+    case TxPIDSettings::PIDS_ROLLATTITUDERESP:
+    case TxPIDSettings::PIDS_PITCHATTITUDERESP:
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
+    case TxPIDSettings::PIDS_YAWATTITUDERESP:
+        return true;
+
+    default:
+        return false;
+    }
+}
+
 template <class StabilizationSettingsBankX>
 static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, int pidOption)
 {
@@ -151,6 +200,18 @@ static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, in
     case TxPIDSettings::PIDS_YAWRATEILIMIT:
         return bank->getYawRatePID_ILimit();
 
+    case TxPIDSettings::PIDS_ROLLRATERESP:
+        return bank->getManualRate_Roll();
+
+    case TxPIDSettings::PIDS_PITCHRATERESP:
+        return bank->getManualRate_Pitch();
+
+    case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
+        return bank->getManualRate_Roll();
+
+    case TxPIDSettings::PIDS_YAWRATERESP:
+        return bank->getManualRate_Yaw();
+
     case TxPIDSettings::PIDS_ROLLATTITUDEKP:
         return bank->getRollPI_Kp();
 
@@ -187,8 +248,23 @@ static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, in
     case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
         return bank->getYawPI_ILimit();
 
+    case TxPIDSettings::PIDS_ROLLATTITUDERESP:
+        return (float)bank->getRollMax();
+
+    case TxPIDSettings::PIDS_PITCHATTITUDERESP:
+        return (float)bank->getPitchMax();
+
+    case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
+        return (float)bank->getRollMax();
+
+    case TxPIDSettings::PIDS_YAWATTITUDERESP:
+        return bank->getYawMax();
+
+    case -1: // The PID Option field was uninitialized.
+        return 0.0f;
+
     default:
-        qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption;
+        Q_ASSERT_X(false, "getDefaultValueForOption", "Incorrect PID option");
         return 0.0f;
     }
 }
@@ -200,7 +276,14 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption)
         return stab->getGyroTau();
     }
 
-    uint bankNumber = m_txpid->pidBank->currentIndex() + 1;
+    int pidBankIndex = m_txpid->pidBank->currentIndex();
+
+    if (pidBankIndex == -1) {
+        // The pidBank field was uninitilized.
+        return 0.0f;
+    }
+
+    int bankNumber = pidBankIndex + 1;
 
     if (bankNumber == 1) {
         StabilizationSettingsBank1 *bank = qobject_cast<StabilizationSettingsBank1 *>(getObject(QString("StabilizationSettingsBank1")));
@@ -212,7 +295,7 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption)
         StabilizationSettingsBank3 *bank = qobject_cast<StabilizationSettingsBank3 *>(getObject(QString("StabilizationSettingsBank3")));
         return defaultValueForPidOption(bank, pidOption);
     } else {
-        qDebug() << "getDefaultValueForPidOption: Incorrect bank number:" << bankNumber;
+        Q_ASSERT_X(false, "getDefaultValueForPidOption", "Incorrect bank number");
         return 0.0f;
     }
 }
@@ -234,10 +317,35 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(int selectedPidOption)
         minPID = m_txpid->MinPID3;
         maxPID = m_txpid->MaxPID3;
     } else {
-        qDebug() << "updateSpinBoxProperties: Incorrect sender object";
+        Q_ASSERT_X(false, "updateSpinBoxProperties", "Incorrect sender object");
         return;
     }
 
+    // The ranges need to be setup before the values can be set,
+    // otherwise the value might be incorrectly capped.
+
+    if (isResponsivenessOption(selectedPidOption)) {
+        if (isAttitudeOption(selectedPidOption)) {
+            // Limit to 180 degrees.
+            minPID->setRange(0, 180);
+            maxPID->setRange(0, 180);
+        } else {
+            minPID->setRange(0, 999);
+            maxPID->setRange(0, 999);
+        }
+        minPID->setSingleStep(1);
+        maxPID->setSingleStep(1);
+        minPID->setDecimals(0);
+        maxPID->setDecimals(0);
+    } else {
+        minPID->setRange(0, 99.99);
+        maxPID->setRange(0, 99.99);
+        minPID->setSingleStep(0.000100);
+        maxPID->setSingleStep(0.000100);
+        minPID->setDecimals(6);
+        maxPID->setDecimals(6);
+    }
+
     float value = getDefaultValueForPidOption(selectedPidOption);
 
     minPID->setValue(value);

shared/uavobjectdefinition/txpidsettings.xml

@@ -15,9 +15,11 @@
 			Roll Rate.Ki, Pitch Rate.Ki, Roll+Pitch Rate.Ki, Yaw Rate.Ki,
 			Roll Rate.Kd, Pitch Rate.Kd, Roll+Pitch Rate.Kd, Yaw Rate.Kd,
 			Roll Rate.ILimit, Pitch Rate.ILimit, Roll+Pitch Rate.ILimit, Yaw Rate.ILimit,
+			Roll Rate.Resp, Pitch Rate.Resp, Roll+Pitch Rate.Resp, Yaw Rate.Resp,
 			Roll Attitude.Kp, Pitch Attitude.Kp, Roll+Pitch Attitude.Kp, Yaw Attitude.Kp,
 			Roll Attitude.Ki, Pitch Attitude.Ki, Roll+Pitch Attitude.Ki, Yaw Attitude.Ki,
 			Roll Attitude.ILimit, Pitch Attitude.ILimit, Roll+Pitch Attitude.ILimit, Yaw Attitude.ILimit,
+			Roll Attitude.Resp, Pitch Attitude.Resp, Roll+Pitch Attitude.Resp, Yaw Attitude.Resp,
 			GyroTau"
 		defaultvalue="Disabled"/>
         <field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>