From ab7ff56d96ec02ce8b96b042e3dda535373ad949 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 19:03:35 -0500 Subject: [PATCH] Compute initial gyro bias while calibrating to speed up convergence --- flight/Modules/Attitude/attitude.c | 2 +- .../plugins/config/configccattitudewidget.cpp | 24 +++++++++++++++---- .../plugins/config/configccattitudewidget.h | 1 + 3 files changed, 22 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index b22b7ec58..49cc54f8b 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -276,7 +276,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw) accel_err[1] /= 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[1] += accel_err[1] * accelKi; //gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT; diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 52488da19..72e59d236 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -79,7 +79,10 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { x_accum.append(field->getDouble(0)); y_accum.append(field->getDouble(1)); 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 ) { updates++; timer.stop(); @@ -90,6 +93,9 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { float y_bias = listMean(y_accum) / 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); UAVDataObject * settings = dynamic_cast(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(1) + y_bias,1); field->setDouble(field->getDouble(2) + z_bias,2); - qDebug("New X bias: %f\n", field->getDouble(0)+x_bias); - qDebug("New Y bias: %f\n", field->getDouble(1)+y_bias); - qDebug("New Z bias: %f\n", field->getDouble(2)+z_bias); + field = settings->getField("GyroBias"); + field->setDouble(x_gyro_bias,0); + field->setDouble(y_gyro_bias,1); + field->setDouble(z_gyro_bias,2); + settings->getField("BiasCorrectedGyro")->setValue("True"); settings->updated(); ui->status->setText("Calibration done."); } else { @@ -159,9 +167,17 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { x_accum.clear(); y_accum.clear(); z_accum.clear(); + x_gyro_accum.clear(); + y_gyro_accum.clear(); + z_gyro_accum.clear(); ui->status->setText(tr("Calibrating...")); + // Disable gyro bias correction to see raw data + UAVDataObject * settings = dynamic_cast(getObjectManager()->getObject(QString("AttitudeSettings"))); + settings->getField("BiasCorrectedGyro")->setValue("False"); + settings->updated(); + // Set up to receive updates UAVDataObject * obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index 1dee369f7..54497880d 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -64,6 +64,7 @@ private: int updates; QList x_accum, y_accum, z_accum; + QList x_gyro_accum, y_gyro_accum, z_gyro_accum; static const int NUM_ACCEL_UPDATES = 60; static const float ACCEL_SCALE = 0.004f * 9.81f;