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 @@ - - - + + +