From 28501842c3174bf0d5fcc636536e75cca120f90d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 22 Jun 2011 19:06:39 -0500 Subject: [PATCH] Expose the rate D term which in simulations seems to help damp oscillations for high RateKp terms. However it might be sensitive to gyro noise (vibrations). In addition it is mathematically similar to lead shapign so probably only use one or the other. --- flight/Modules/Stabilization/stabilization.c | 39 +++++++++++++++---- .../config/configstabilizationwidget.cpp | 36 ++++++++--------- .../stabilizationsettings.xml | 6 +-- 3 files changed, 53 insertions(+), 28 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 581cbe747..ef93e961d 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -336,13 +336,38 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) memset(pids,0,sizeof (pid_type) * PID_MAX); StabilizationSettingsGet(&settings); - float * data = settings.RollRatePI; - for(int8_t pid=0; pid < PID_MAX; pid++) - { - pids[pid].p = *data++; - pids[pid].i = *data++; - pids[pid].iLim = *data++; - } + // Set the roll rate PID constants + pids[0].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; + pids[0].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; + pids[0].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; + pids[0].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; + + // Set the pitch rate PID constants + pids[1].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; + pids[1].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; + pids[1].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; + pids[1].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; + + // Set the yaw rate PID constants + pids[2].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; + pids[2].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; + pids[2].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; + pids[2].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; + + // Set the roll attitude PI constants + pids[3].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; + pids[3].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; + pids[3].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; + + // Set the pitch attitude PI constants + pids[4].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; + pids[4].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; + pids[4].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; + + // Set the yaw attitude PI constants + pids[5].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; + pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; + pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index f691cc22f..83ac65523 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -194,17 +194,17 @@ void ConfigStabilizationWidget::refreshValues() // stabSettings->requestUpdate(); StabilizationSettings::DataFields stabData = stabSettings->getData(); // Now fill in all the fields, this is fairly tedious: - m_stabilization->rateRollKp->setValue(stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_KP]); - m_stabilization->rateRollKi->setValue(stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_KI]); - m_stabilization->rateRollILimit->setValue(stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_ILIMIT]); + m_stabilization->rateRollKp->setValue(stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]); + m_stabilization->rateRollKi->setValue(stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]); + m_stabilization->rateRollILimit->setValue(stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_ILIMIT]); - m_stabilization->ratePitchKp->setValue(stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_KP]); - m_stabilization->ratePitchKi->setValue(stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_KI]); - m_stabilization->ratePitchILimit->setValue(stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_ILIMIT]); + m_stabilization->ratePitchKp->setValue(stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP]); + m_stabilization->ratePitchKi->setValue(stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI]); + m_stabilization->ratePitchILimit->setValue(stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_ILIMIT]); - m_stabilization->rateYawKp->setValue(stabData.YawRatePI[StabilizationSettings::YAWRATEPI_KP]); - m_stabilization->rateYawKi->setValue(stabData.YawRatePI[StabilizationSettings::YAWRATEPI_KI]); - m_stabilization->rateYawILimit->setValue(stabData.YawRatePI[StabilizationSettings::YAWRATEPI_ILIMIT]); + m_stabilization->rateYawKp->setValue(stabData.YawRatePID[StabilizationSettings::YAWRATEPID_KP]); + m_stabilization->rateYawKi->setValue(stabData.YawRatePID[StabilizationSettings::YAWRATEPID_KI]); + m_stabilization->rateYawILimit->setValue(stabData.YawRatePID[StabilizationSettings::YAWRATEPID_ILIMIT]); m_stabilization->rollKp->setValue(stabData.RollPI[StabilizationSettings::ROLLPI_KP]); m_stabilization->rollKi->setValue(stabData.RollPI[StabilizationSettings::ROLLPI_KI]); @@ -241,17 +241,17 @@ void ConfigStabilizationWidget::sendStabilizationUpdate() { StabilizationSettings::DataFields stabData = stabSettings->getData(); - stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_KP] = m_stabilization->rateRollKp->value(); - stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_KI] = m_stabilization->rateRollKi->value(); - stabData.RollRatePI[StabilizationSettings::ROLLRATEPI_ILIMIT] = m_stabilization->rateRollILimit->value(); + stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = m_stabilization->rateRollKp->value(); + stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = m_stabilization->rateRollKi->value(); + stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_ILIMIT] = m_stabilization->rateRollILimit->value(); - stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_KP] = m_stabilization->ratePitchKp->value(); - stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_KI] = m_stabilization->ratePitchKi->value(); - stabData.PitchRatePI[StabilizationSettings::PITCHRATEPI_ILIMIT] = m_stabilization->ratePitchILimit->value(); + stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = m_stabilization->ratePitchKp->value(); + stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = m_stabilization->ratePitchKi->value(); + stabData.PitchRatePID[StabilizationSettings::PITCHRATEPID_ILIMIT] = m_stabilization->ratePitchILimit->value(); - stabData.YawRatePI[StabilizationSettings::YAWRATEPI_KP] = m_stabilization->rateYawKp->value(); - stabData.YawRatePI[StabilizationSettings::YAWRATEPI_KI] = m_stabilization->rateYawKi->value(); - stabData.YawRatePI[StabilizationSettings::YAWRATEPI_ILIMIT] = m_stabilization->rateYawILimit->value(); + stabData.YawRatePID[StabilizationSettings::YAWRATEPID_KP] = m_stabilization->rateYawKp->value(); + stabData.YawRatePID[StabilizationSettings::YAWRATEPID_KI] = m_stabilization->rateYawKi->value(); + stabData.YawRatePID[StabilizationSettings::YAWRATEPID_ILIMIT] = m_stabilization->rateYawILimit->value(); stabData.RollPI[StabilizationSettings::ROLLPI_KP] = m_stabilization->rollKp->value(); stabData.RollPI[StabilizationSettings::ROLLPI_KI] = m_stabilization->rollKi->value(); diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 4eac63d93..39183f372 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -7,9 +7,9 @@ - - - + + +