diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index a48a20967..f356558e8 100644 --- a/flight/modules/TxPID/txpid.c +++ b/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; +} + /** * @} */ diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 3facff7fa..0f0e7fa87 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/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 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(getObject(QString("StabilizationSettingsBank1"))); @@ -212,7 +295,7 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption) StabilizationSettingsBank3 *bank = qobject_cast(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); diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 9f1a91579..e85774e9b 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/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"/>