rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Include initial gyro bias calibration into the level calibration of revo like

Browse Source 
it is with CC.
This commit is contained in:
James Cotton 2012-06-13 23:07:23 -05:00
parent 03705771dd
commit 70606a46a1
3 changed files with 98 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/Modules/Sensors/sensors.c
View File

@ -88,6 +88,7 @@ static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0};
static float gyro_bias[3] = {0,0,0};
static float R[3][3] = {{0}};
static int8_t rotate = 0;
@ -379,9 +380,9 @@ static void SensorsTask(void *parameters)
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
gyrosData.x += gyrosBias.x - gyro_bias[0];
gyrosData.y += gyrosBias.y - gyro_bias[1];
gyrosData.z += gyrosBias.z - gyro_bias[2];
}
GyrosSet(&gyrosData);
@ -448,9 +449,13 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
gyro_bias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_bias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_bias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
bias_correct_gyro = (attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE);
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&

120
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -40,6 +40,7 @@
#include <iostream>
#include <QDesktopServices>
#include <QUrl>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <homelocation.h>
#include <accels.h>
@ -214,15 +215,10 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
autoLoadWidgets();
// Connect the signals
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(doStartAccelGyroBiasCalibration()));
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibration()));
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ui->noiseMeasurementStart, SIGNAL(clicked()), this, SLOT(doStartNoiseMeasurement()));
// Leave this timer permanently connected. The timer itself is started and stopped.
connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress()));
// Connect the help button
}
ConfigRevoWidget::~ConfigRevoWidget()
@ -248,11 +244,10 @@ void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
m_ui->sixPointsHelp->fitInView(paperplane,Qt::KeepAspectRatio);
}
/**
Starts an accelerometer bias calibration.
* Starts an accelerometer bias calibration.
*/
void ConfigRevoWidget::launchAccelBiasCalibration()
void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
{
m_ui->accelBiasStart->setEnabled(false);
m_ui->accelBiasProgress->setValue(0);
@ -264,47 +259,98 @@ void ConfigRevoWidget::launchAccelBiasCalibration()
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings * attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
UAVObject::Metadata mdata;
/* Need to get as many accel updates as possible */
Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
initialAccelsMdata = accels->getMetadata();
UAVObject::Metadata mdata = initialAccelsMdata;
mdata = initialAccelsMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata);
Gyros * gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
initialGyrosMdata = gyros->getMetadata();
mdata = initialGyrosMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyros->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetAccelBiasData(UAVObject*)));
connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetAccelGyroBiasData(UAVObject*)));
connect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetAccelGyroBiasData(UAVObject*)));
}
/**
Updates the accel bias raw values
*/
void ConfigRevoWidget::doGetAccelBiasData(UAVObject *obj)
void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
{
Q_UNUSED(obj);
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Accels::DataFields accelsData = accels->getData();
switch(obj->getObjID()) {
case Accels::OBJID:
{
Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Accels::DataFields accelsData = accels->getData();
// This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) {
accel_accum_x.append(accelsData.x);
accel_accum_y.append(accelsData.y);
accel_accum_z.append(accelsData.z);
break;
}
case Gyros::OBJID:
{
Gyros * gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
Gyros::DataFields gyrosData = gyros->getData();
gyro_accum_x.append(gyrosData.x);
gyro_accum_y.append(gyrosData.y);
gyro_accum_z.append(gyrosData.z);
break;
}
default:
Q_ASSERT(0);
}
m_ui->accelBiasProgress->setValue(m_ui->accelBiasProgress->value()+1);
// Work out the progress based on whichever has less
double p1 = (double) accel_accum_x.size() / (double) NOISE_SAMPLES;
double p2 = (double) accel_accum_y.size() / (double) NOISE_SAMPLES;
m_ui->accelBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100);
if(accel_accum_x.size() >= NOISE_SAMPLES &&
gyro_accum_y.size() >= NOISE_SAMPLES &&
collectingData == true) {
if(accel_accum_x.size() >= 100 && collectingData == true) {
collectingData = false;
disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetAccelBiasData(UAVObject*)));
Accels * accels = Accels::GetInstance(getObjectManager());
Gyros * gyros = Gyros::GetInstance(getObjectManager());
disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetAccelGyroBiasData(UAVObject*)));
disconnect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetAccelGyroBiasData(UAVObject*)));
m_ui->accelBiasStart->setEnabled(true);
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
@ -312,34 +358,26 @@ void ConfigRevoWidget::doGetAccelBiasData(UAVObject *obj)
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update the biases based on collected data
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += ( listMean(accel_accum_z) + GRAVITY );
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = listMean(gyro_accum_x);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = listMean(gyro_accum_y);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = listMean(gyro_accum_z);
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
AttitudeSettings * attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accels->setMetadata(initialAccelsMdata);
}
}
/**
Increment progress bar for noise measurements (not really based on feedback)
*/
void ConfigRevoWidget::incrementProgress()
{
m_ui->noiseMeasurementProgress->setValue(m_ui->noiseMeasurementProgress->value()+1);
if (m_ui->noiseMeasurementProgress->value() >= m_ui->noiseMeasurementProgress->maximum()) {
progressBarTimer.stop();
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
disconnect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
collectingData = false;
QErrorMessage err(this);
err.showMessage("Noise measurement timed out. State undetermined. Please power cycle.");
err.exec();
gyros->setMetadata(initialGyrosMdata);
}
}

19
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -55,6 +55,9 @@ private:
void drawVariancesGraph();
void displayPlane(QString elementID);
//! Computes the scale and bias of the mag based on collected data
void computeScaleBias();
Ui_RevoSensorsWidget *m_ui;
QGraphicsSvgItem *paperplane;
QGraphicsSvgItem *sensorsBargraph;
@ -70,8 +73,6 @@ private:
QMutex sensorsUpdateLock;
double maxBarHeight;
int phaseCounter;
int progressBarIndex;
QTimer progressBarTimer;
const static double maxVarValue;
const static int calibrationDelay = 10;
@ -100,15 +101,17 @@ private:
static const int NOISE_SAMPLES = 100;
private slots:
void launchAccelBiasCalibration();
void incrementProgress();
//! Overriden method from the configTaskWidget to update UI
virtual void refreshWidgetsValues(UAVObject * obj=NULL);
void savePositionData();
void computeScaleBias();
// Slots for calibrating the mags
void doStartSixPointCalibration();
void doGetSixPointCalibrationMeasurement(UAVObject * obj);
void doGetAccelBiasData(UAVObject*);
void savePositionData();
// Slots for calibrating the accel and gyro
void doStartAccelGyroBiasCalibration();
void doGetAccelGyroBiasData(UAVObject*);
// Slots for measuring the sensor noise
void doStartNoiseMeasurement();