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

Merge branch 'next' into revo-mini

Browse Source
This commit is contained in:
James Cotton 2012-09-12 02:03:39 -05:00
commit 223a3473c2
6 changed files with 106 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
flight/Libraries/math/pid.c
View File

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

2
flight/Libraries/math/pid.h
View File

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

11
flight/Modules/Stabilization/stabilization.c
View File

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

53
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

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

4
ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
View File

@ -34,7 +34,6 @@
#include "uavobject.h"
#include <QtGui/QWidget>
#include <QTimer>
#include <QMutex>
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<double> x_accum, y_accum, z_accum;
QList<double> 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);

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -24,6 +24,8 @@
<field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="15"/>
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>