rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

Compute initial gyro bias while calibrating to speed up convergence

Browse Source
This commit is contained in:
James Cotton 2011-06-23 19:03:35 -05:00
parent c0eff41dc6
commit ab7ff56d96
3 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/Modules/Attitude/attitude.c
View File

@ -276,7 +276,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
accel_err[1] /= accel_mag; accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag; accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (rad/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi; gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi; gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT; //gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;

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

@ -79,7 +79,10 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) {
x_accum.append(field->getDouble(0)); x_accum.append(field->getDouble(0));
y_accum.append(field->getDouble(1)); y_accum.append(field->getDouble(1));
z_accum.append(field->getDouble(2)); z_accum.append(field->getDouble(2));
qDebug("update %d: %f, %f, %f\n",updates,field->getDouble(0),field->getDouble(1),field->getDouble(2)); field = obj->getField(QString("gyros"));
x_gyro_accum.append(field->getDouble(0));
y_gyro_accum.append(field->getDouble(1));
z_gyro_accum.append(field->getDouble(2));;
} else if ( updates == NUM_ACCEL_UPDATES ) { } else if ( updates == NUM_ACCEL_UPDATES ) {
updates++; updates++;
timer.stop(); timer.stop();
@ -90,6 +93,9 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) {
float y_bias = listMean(y_accum) / ACCEL_SCALE; float y_bias = listMean(y_accum) / ACCEL_SCALE;
float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE; float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE;
float x_gyro_bias = listMean(x_gyro_accum);
float y_gyro_bias = listMean(y_gyro_accum);
float z_gyro_bias = listMean(z_gyro_accum);
obj->setMetadata(initialMdata); obj->setMetadata(initialMdata);
UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings"))); UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
@ -97,9 +103,11 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) {
field->setDouble(field->getDouble(0) + x_bias,0); field->setDouble(field->getDouble(0) + x_bias,0);
field->setDouble(field->getDouble(1) + y_bias,1); field->setDouble(field->getDouble(1) + y_bias,1);
field->setDouble(field->getDouble(2) + z_bias,2); field->setDouble(field->getDouble(2) + z_bias,2);
qDebug("New X bias: %f\n", field->getDouble(0)+x_bias); field = settings->getField("GyroBias");
qDebug("New Y bias: %f\n", field->getDouble(1)+y_bias); field->setDouble(x_gyro_bias,0);
qDebug("New Z bias: %f\n", field->getDouble(2)+z_bias); field->setDouble(y_gyro_bias,1);
field->setDouble(z_gyro_bias,2);
settings->getField("BiasCorrectedGyro")->setValue("True");
settings->updated(); settings->updated();
ui->status->setText("Calibration done."); ui->status->setText("Calibration done.");
} else { } else {
@ -159,9 +167,17 @@ void ConfigCCAttitudeWidget::startAccelCalibration() {
x_accum.clear(); x_accum.clear();
y_accum.clear(); y_accum.clear();
z_accum.clear(); z_accum.clear();
x_gyro_accum.clear();
y_gyro_accum.clear();
z_gyro_accum.clear();
ui->status->setText(tr("Calibrating...")); ui->status->setText(tr("Calibrating..."));
// Disable gyro bias correction to see raw data
UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
settings->getField("BiasCorrectedGyro")->setValue("False");
settings->updated();
// Set up to receive updates // Set up to receive updates
UAVDataObject * obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw"))); UAVDataObject * obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));

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

@ -64,6 +64,7 @@ private:
int updates; int updates;
QList<double> x_accum, y_accum, z_accum; QList<double> x_accum, y_accum, z_accum;
QList<double> x_gyro_accum, y_gyro_accum, z_gyro_accum;
static const int NUM_ACCEL_UPDATES = 60; static const int NUM_ACCEL_UPDATES = 60;
static const float ACCEL_SCALE = 0.004f * 9.81f; static const float ACCEL_SCALE = 0.004f * 9.81f;