diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c index 8bb0e6013..1df416123 100644 --- a/flight/Libraries/math/pid.c +++ b/flight/Libraries/math/pid.c @@ -31,23 +31,69 @@ #include "openpilot.h" #include "pid.h" +#define F_PI ((float) M_PI) + //! Private method static float bound(float val, float range); +//! Store the shared time constant for the derivative cutoff. +static float deriv_tau = 7.9577e-3f; + +//! Store the setpoint weight to apply for the derivative term +static float deriv_gamma = 1.0; + +/** + * Update the PID computation + * @param[in] pid The PID struture which stores temporary information + * @param[in] err The error term + * @param[in] dT The time step + * @returns Output the computed controller value + */ float pid_apply(struct pid *pid, const float err, float dT) -{ +{ + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + + // Calculate DT1 term float diff = (err - pid->lastErr); float dterm = 0; pid->lastErr = err; + if(pid->d && dT) + { + dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); +} + +/** + * Update the PID computation with setpoint weighting on the derivative + * @param[in] pid The PID struture which stores temporary information + * @param[in] setpoint The setpoint to use + * @param[in] measured The measured value of output + * @param[in] dT The time step + * @returns Output the computed controller value + * + * This version of apply uses setpoint weighting for the derivative component so the gain + * on the gyro derivative can be different than the gain on the setpoint derivative + */ +float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT) +{ + float err = setpoint - measured; // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); - // Calculate DT1 term, fixed T1 timeconstant + // Calculate DT1 term, + float dterm = 0; + float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); + pid->lastErr = (deriv_gamma * setpoint - measured); if(pid->d && dT) { - dterm = pid->lastDer + dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer); + dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) } // 7.9577e-3 means 20 Hz f_cutoff @@ -68,6 +114,17 @@ void pid_zero(struct pid *pid) pid->lastDer = 0; } +/** + * @brief Configure the common terms that alter ther derivative + * @param[in] cutoff The cutoff frequency (in Hz) + * @param[in] gamma The gamma term for setpoint shaping (unsused now) + */ +void pid_configure_derivative(float cutoff, float g) +{ + deriv_tau = 1.0f / (2 * F_PI * cutoff); + deriv_gamma = g; +} + /** * Configure the settings for a pid structure * @param[out] pid The PID structure to configure diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h index e817cab57..602f560bf 100644 --- a/flight/Libraries/math/pid.h +++ b/flight/Libraries/math/pid.h @@ -44,7 +44,9 @@ struct pid { //! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); +float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); +void pid_configure_derivative(float cutoff, float gamma); #endif /* PID_H */ \ No newline at end of file diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 9aabff114..fc4a5dcc5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -247,7 +247,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -263,7 +263,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -287,7 +287,7 @@ static void stabilizationTask(void* parameters) // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -309,7 +309,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -448,6 +448,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + // Set up the derivative term + pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); + // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; max_axislock_rate = settings.MaxAxisLockRate; diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index fe1f517bf..2131f58ea 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -43,7 +43,6 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : ui->setupUi(this); connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); - addApplySaveButtons(ui->applyButton,ui->saveButton); addUAVObject("AttitudeSettings"); @@ -64,26 +63,39 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() } void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { - QMutexLocker locker(&startStop); - ui->zeroBiasProgress->setValue((float) qMin(accelUpdates,gyroUpdates) / NUM_SENSOR_UPDATES * 100); + if (!timer.isActive()) { + // ignore updates that come in after the timer has expired + return; + } Accels * accels = Accels::GetInstance(getObjectManager()); Gyros * gyros = Gyros::GetInstance(getObjectManager()); - if(obj->getObjID() == Accels::OBJID && accelUpdates < NUM_SENSOR_UPDATES) { + // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples + // for both gyros and accels. + // Note that, at present, we stash the samples and then compute the bias + // at the end, even though the mean could be accumulated as we go. + // In future, a better algorithm could be used. + if(obj->getObjID() == Accels::OBJID) { accelUpdates++; Accels::DataFields accelsData = accels->getData(); x_accum.append(accelsData.x); y_accum.append(accelsData.y); z_accum.append(accelsData.z); - } else if (obj->getObjID() == Gyros::OBJID && gyroUpdates < NUM_SENSOR_UPDATES) { + } else if (obj->getObjID() == Gyros::OBJID) { gyroUpdates++; Gyros::DataFields gyrosData = gyros->getData(); x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } else if ( accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { + } + + // update the progress indicator + ui->zeroBiasProgress->setValue((float) qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + + // If we have enough samples, then stop sampling and compute the biases + if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -108,14 +120,13 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { attitudeSettingsData.GyroBias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); - } else { - // Possible to get here if weird threading stuff happens. Just ignore updates. - qDebug("Unexpected accel update received."); + + // reenable controls + enableControls(true); } } void ConfigCCAttitudeWidget::timeout() { - QMutexLocker locker(&startStop); UAVDataObject * obj = Accels::GetInstance(getObjectManager()); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -131,10 +142,15 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); + // reset progress indicator + ui->zeroBiasProgress->setValue(0); + // reenable controls + enableControls(true); } void ConfigCCAttitudeWidget::startAccelCalibration() { - QMutexLocker locker(&startStop); + // disable controls during sampling + enableControls(false); accelUpdates = 0; gyroUpdates = 0; @@ -156,28 +172,28 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { connect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); connect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - // Set up timeout timer - timer.start(10000); - connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); - // Speed up updates initialAccelsMdata = accels->getMetadata(); UAVObject::Metadata accelsMdata = initialAccelsMdata; UAVObject::SetFlightTelemetryUpdateMode(accelsMdata, UAVObject::UPDATEMODE_PERIODIC); - accelsMdata.flightTelemetryUpdatePeriod = 30; + accelsMdata.flightTelemetryUpdatePeriod = 30; // ms accels->setMetadata(accelsMdata); initialGyrosMdata = gyros->getMetadata(); UAVObject::Metadata gyrosMdata = initialGyrosMdata; UAVObject::SetFlightTelemetryUpdateMode(gyrosMdata, UAVObject::UPDATEMODE_PERIODIC); - gyrosMdata.flightTelemetryUpdatePeriod = 30; + gyrosMdata.flightTelemetryUpdatePeriod = 30; // ms gyros->setMetadata(gyrosMdata); + // Set up timeout timer + timer.setSingleShot(true); + timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, + gyrosMdata.flightTelemetryUpdatePeriod))); + connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); } void ConfigCCAttitudeWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/CopterControl+Attitude+Configuration", QUrl::StrictMode) ); } @@ -186,7 +202,6 @@ void ConfigCCAttitudeWidget::enableControls(bool enable) if(ui->zeroBias) ui->zeroBias->setEnabled(enable); ConfigTaskWidget::enableControls(enable); - } void ConfigCCAttitudeWidget::updateObjectsFromWidgets() diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index fc1a7f623..0637d268a 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -34,7 +34,6 @@ #include "uavobject.h" #include #include -#include class Ui_Widget; @@ -55,7 +54,6 @@ private slots: void openHelp(); private: - QMutex startStop; Ui_ccattitude *ui; QTimer timer; UAVObject::Metadata initialAccelsMdata; @@ -67,7 +65,7 @@ private: QList x_accum, y_accum, z_accum; QList x_gyro_accum, y_gyro_accum, z_gyro_accum; - static const int NUM_SENSOR_UPDATES = 60; + static const int NUM_SENSOR_UPDATES = 300; static const float ACCEL_SCALE = 0.004f * 9.81f; protected: virtual void enableControls(bool enable); diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 7cd7de210..b1b541310 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -24,6 +24,8 @@ + +