rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

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

Browse Source
This commit is contained in:
Fredrik Arvidsson 2014-08-12 23:07:14 +02:00
commit 2bdd4d6d08
3 changed files with 160 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
flight/modules/TxPID/txpid.c
View File

@ -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;
}
/**
* @}
*/

124
ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp
View File

@ -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);

2
shared/uavobjectdefinition/txpidsettings.xml
View File

@ -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"/>