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

INS: Rework how the sensor calibration behaves and clean up the interface a

Browse Source 
bit.
This commit is contained in:
James Cotton 2011-09-01 03:56:59 -05:00
parent 3b20df22d1
commit 1d04715476
4 changed files with 181 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/INS/inc/ins.h
View File

@ -62,7 +62,8 @@ struct accel_sensor {
float z; float z;
} filtered; } filtered;
struct { struct {
float scale[3][4]; float bias[3];
float scale[3];
float variance[3]; float variance[3];
} calibration; } calibration;
}; };

286
flight/INS/ins.c
View File

@ -71,7 +71,7 @@ void get_baro_data();
void get_accel_gyro_data(); void get_accel_gyro_data();
void reset_values(); void reset_values();
void calibrate_sensors(void); void measure_noise(void);
/* Communication functions */ /* Communication functions */
//void send_calibration(void); //void send_calibration(void);
@ -193,7 +193,9 @@ int main()
for all data to be up to date before doing anything*/ for all data to be up to date before doing anything*/
InsSettingsConnectCallback(settings_callback); InsSettingsConnectCallback(settings_callback);
HomeLocationConnectCallback(homelocation_callback); HomeLocationConnectCallback(homelocation_callback);
FirmwareIAPObjConnectCallback(firmwareiapobj_callback); //FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
settings_callback(InsSettingsHandle());
/******************* Main EKF loop ****************************/ /******************* Main EKF loop ****************************/
while(1) { while(1) {
@ -243,6 +245,9 @@ int main()
case INSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG: case INSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
ins_indoor_update(); ins_indoor_update();
break; break;
case INSSETTINGS_ALGORITHM_CALIBRATION:
measure_noise();
break;
} }
status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2); status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2);
@ -485,90 +490,73 @@ void get_baro_data()
* sense for the z accel so make sure 6 point calibration is also run and those values set * sense for the z accel so make sure 6 point calibration is also run and those values set
* after these read. * after these read.
*/ */
#define NBIAS 100 #define NMEAN 500
#define NVAR 500 #define NVAR 1000
void calibrate_sensors() #define CHANNELS 6
static uint32_t calibrate_count = 0;
float f_means[CHANNELS];
float f_var[CHANNELS] = {0, 0, 0, 0, 0, 0};
void measure_noise()
{ {
int i,j; uint32_t i;
float accel_bias[3] = {0, 0, 0};
float gyro_bias[3] = {0, 0, 0};
float mag_bias[3] = {0, 0, 0};
float data[CHANNELS] = {accel_data.filtered.x,
accel_data.filtered.y,
accel_data.filtered.z,
gyro_data.filtered.x,
gyro_data.filtered.y,
gyro_data.filtered.z
};
for (i = 0, j = 0; i < NBIAS; i++) { // First step, zero all sufficient statistics
if(calibrate_count == 0) {
get_accel_gyro_data(); for (i = 0; i < CHANNELS; i++) {
f_means[i] = 0;
gyro_bias[0] += gyro_data.filtered.x / NBIAS; f_var[i] = 0;
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
accel_bias[0] += accel_data.filtered.x / NBIAS;
accel_bias[1] += accel_data.filtered.y / NBIAS;
accel_bias[2] += accel_data.filtered.z / NBIAS;
#if defined(PIOS_INCLUDE_HMC5883) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5883_NewDataAvailable()) {
j ++;
PIOS_HMC5883_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_bias[0] += mag_data.scaled.axis[0];
mag_bias[1] += mag_data.scaled.axis[1];
mag_bias[2] += mag_data.scaled.axis[2];
} }
#endif
}
mag_bias[0] /= j;
mag_bias[1] /= j;
mag_bias[2] /= j;
gyro_data.calibration.variance[0] = 0;
gyro_data.calibration.variance[1] = 0;
gyro_data.calibration.variance[2] = 0;
mag_data.calibration.variance[0] = 0;
mag_data.calibration.variance[1] = 0;
mag_data.calibration.variance[2] = 0;
accel_data.calibration.variance[0] = 0;
accel_data.calibration.variance[1] = 0;
accel_data.calibration.variance[2] = 0;
for (i = 0, j = 0; j < NVAR; j++) {
get_accel_gyro_data();
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
#if defined(PIOS_INCLUDE_HMC5883) && defined(PIOS_INCLUDE_I2C)
if(PIOS_HMC5883_NewDataAvailable()) {
j ++;
PIOS_HMC5883_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
}
#endif
} }
mag_data.calibration.variance[0] /= j; // Accumulate for an estimate of mean
mag_data.calibration.variance[1] /= j; if(calibrate_count < NMEAN)
mag_data.calibration.variance[2] /= j; for (i = 0; i < CHANNELS; i++)
f_means[i] += data[i];
if(calibrate_count == NMEAN)
for (i = 0; i < CHANNELS; i++)
f_means[i] /= (float) NMEAN;
// Accumulate for estimate of variance. This needs to be done
// sequentially because storing second moment would go out of
// float precision
if(calibrate_count >= NMEAN && calibrate_count < (NMEAN + NVAR))
for (i = 0; i < CHANNELS; i++)
f_var[i] += pow(f_means[i] - data[i],2);
if(calibrate_count == (NMEAN + NVAR)) {
for (i = 0; i < CHANNELS; i++)
f_var[i] /= (float) (NVAR - 1);
calibrate_count = 0;
InsSettingsData settings;
InsSettingsGet(&settings);
settings.Algorithm = INSSETTINGS_ALGORITHM_NONE;
settings.accel_var[0] = f_var[0];
settings.accel_var[1] = f_var[1];
settings.accel_var[2] = f_var[2];
settings.gyro_var[0] = f_var[3];
settings.gyro_var[1] = f_var[4];
settings.gyro_var[2] = f_var[5];
InsSettingsSet(&settings);
settings_callback(InsSettingsHandle());
} else {
PIOS_DELAY_WaituS(3300);
calibrate_count++;
}
gyro_data.calibration.bias[0] -= gyro_bias[0];
gyro_data.calibration.bias[1] -= gyro_bias[1];
gyro_data.calibration.bias[2] -= gyro_bias[2];
// Approximately how long the EKF takes to update
PIOS_DELAY_WaituS(1500);
} }
@ -577,33 +565,26 @@ void calibrate_sensors()
*/ */
void reset_values() void reset_values()
{ {
accel_data.calibration.scale[0][1] = 0; accel_data.calibration.scale[0] = 1;
accel_data.calibration.scale[1][0] = 0; accel_data.calibration.scale[1] = 1;
accel_data.calibration.scale[0][2] = 0; accel_data.calibration.scale[2] = 1;
accel_data.calibration.scale[2][0] = 0; accel_data.calibration.bias[0] = 0;
accel_data.calibration.scale[1][2] = 0; accel_data.calibration.bias[1] = 0;
accel_data.calibration.scale[2][1] = 0; accel_data.calibration.bias[2] = 0;
accel_data.calibration.variance[0] = 1;
accel_data.calibration.variance[1] = 1;
accel_data.calibration.variance[2] = 1;
accel_data.calibration.scale[0][0] = 0.0359; gyro_data.calibration.scale[0] = 1;
accel_data.calibration.scale[1][1] = 0.0359; gyro_data.calibration.scale[1] = 1;
accel_data.calibration.scale[2][2] = 0.0359; gyro_data.calibration.scale[2] = 1;
accel_data.calibration.scale[0][3] = -73.5; gyro_data.calibration.bias[0] = 0;
accel_data.calibration.scale[1][3] = -73.5; gyro_data.calibration.bias[1] = 0;
accel_data.calibration.scale[2][3] = -73.5; gyro_data.calibration.bias[2] = 0;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1; gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1; gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1; gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1; mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1; mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1; mag_data.calibration.scale[2] = 1;
@ -659,84 +640,45 @@ void send_position(void)
PositionActualSet(&positionActual); PositionActualSet(&positionActual);
} }
/* int callback_count = 0;
void send_calibration(void)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
for(int ct=0; ct<3; ct++)
{
cal.accel_var[ct] = accel_data.calibration.variance[ct];
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
cal.mag_var[ct] = mag_data.calibration.variance[ct];
}
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
AHRSCalibrationSet(&cal);
}
*/
/**
* @brief INS calibration callback
*
* Called when the OP board sets the calibration
*/
/*
void calibration_callback(AhrsObjHandle obj)
{
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
accel_data.calibration.scale[0][1] = cal.accel_ortho[0];
accel_data.calibration.scale[1][0] = cal.accel_ortho[0];
accel_data.calibration.scale[0][2] = cal.accel_ortho[1];
accel_data.calibration.scale[2][0] = cal.accel_ortho[1];
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];
accel_data.calibration.scale[ct][3] = cal.accel_bias[ct];
accel_data.calibration.variance[ct] = cal.accel_var[ct];
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
}
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
// one in code
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
INSSetMagVar(mag_var);
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
}
else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE) {
calibrate_sensors();
send_calibration();
}
INSSetPosVelVar(cal.pos_var, cal.vel_var);
}
*/
void settings_callback(AhrsObjHandle obj) void settings_callback(AhrsObjHandle obj)
{ {
callback_count ++;
InsSettingsData settings; InsSettingsData settings;
InsSettingsGet(&settings); InsSettingsGet(&settings);
ahrs_algorithm = settings.Algorithm; ahrs_algorithm = settings.Algorithm;
bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE; bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE;
accel_data.calibration.scale[0] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_X];
accel_data.calibration.scale[1] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Y];
accel_data.calibration.scale[2] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Z];
accel_data.calibration.bias[0] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_X];
accel_data.calibration.bias[1] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Y];
accel_data.calibration.bias[2] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Z];
accel_data.calibration.variance[0] = settings.accel_var[INSSETTINGS_ACCEL_VAR_X];
accel_data.calibration.variance[1] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Y];
accel_data.calibration.variance[2] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Z];
gyro_data.calibration.scale[0] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_X];
gyro_data.calibration.scale[1] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Y];
gyro_data.calibration.scale[2] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Z];
gyro_data.calibration.bias[0] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X];
gyro_data.calibration.bias[1] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y];
gyro_data.calibration.bias[2] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z];
gyro_data.calibration.variance[0] = settings.gyro_var[INSSETTINGS_GYRO_VAR_X];
gyro_data.calibration.variance[1] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Y];
gyro_data.calibration.variance[2] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Z];
mag_data.calibration.scale[0] = settings.mag_scale[INSSETTINGS_MAG_SCALE_X];
mag_data.calibration.scale[1] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Y];
mag_data.calibration.scale[2] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Z];
mag_data.calibration.bias[0] = settings.mag_bias[INSSETTINGS_MAG_BIAS_X];
mag_data.calibration.bias[1] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Y];
mag_data.calibration.bias[2] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Z];
mag_data.calibration.variance[0] = settings.mag_var[INSSETTINGS_MAG_VAR_X];
mag_data.calibration.variance[1] = settings.mag_var[INSSETTINGS_MAG_VAR_Y];
mag_data.calibration.variance[2] = settings.mag_var[INSSETTINGS_MAG_VAR_Z];
} }
void homelocation_callback(AhrsObjHandle obj) void homelocation_callback(AhrsObjHandle obj)

112
ground/openpilotgcs/src/plugins/config/configahrswidget.cpp
View File

@ -35,6 +35,7 @@
#include <QtGui/QVBoxLayout> #include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton> #include <QtGui/QPushButton>
#include <QThread> #include <QThread>
#include <QErrorMessage>
#include <iostream> #include <iostream>
#include <Eigen/align-function.h> #include <Eigen/align-function.h>
#include <QDesktopServices> #include <QDesktopServices>
@ -49,7 +50,6 @@
#define sign(x) ((x < 0) ? -1 : 1) #define sign(x) ((x < 0) ? -1 : 1)
const double ConfigAHRSWidget::maxVarValue = 0.1; const double ConfigAHRSWidget::maxVarValue = 0.1;
const int ConfigAHRSWidget::calibrationDelay = 7; // Time to wait for the AHRS to do its calibration
// ***************** // *****************
@ -221,7 +221,7 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
m_ahrs->sixPointsStart->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE); m_ahrs->sixPointsStart->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE);
// Connect the signals // Connect the signals
connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(launchAHRSCalibration())); connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(measureNoise()));
connect(m_ahrs->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration())); connect(m_ahrs->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
@ -233,6 +233,9 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
connect(m_ahrs->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData())); connect(m_ahrs->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ahrs->startDriftCalib, SIGNAL(clicked()),this, SLOT(launchGyroDriftCalibration())); connect(m_ahrs->startDriftCalib, SIGNAL(clicked()),this, SLOT(launchGyroDriftCalibration()));
// Leave this timer permanently connected. The timer itself is started and stopped.
connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress()));
// Order is important: 1st request the settings (it will also enable the controls) // Order is important: 1st request the settings (it will also enable the controls)
// then explicitely disable them. They will be re-enabled right afterwards by the // then explicitely disable them. They will be re-enabled right afterwards by the
// configgadgetwidget if the autopilot is actually connected. // configgadgetwidget if the autopilot is actually connected.
@ -492,80 +495,76 @@ void ConfigAHRSWidget::computeGyroDrift() {
} }
/** /**
Launches the AHRS sensors calibration Launches the INS sensors noise measurements
*/ */
void ConfigAHRSWidget::launchAHRSCalibration() void ConfigAHRSWidget::measureNoise()
{ {
if(collectingData) {
QErrorMessage err(this);
err.showMessage("Cannot start noise measurement as data already being gathered");
err.exec();
return;
}
m_ahrs->calibInstructions->setText("Estimating sensor variance..."); m_ahrs->calibInstructions->setText("Estimating sensor variance...");
m_ahrs->ahrsCalibStart->setEnabled(false); m_ahrs->ahrsCalibStart->setEnabled(false);
m_ahrs->calibProgress->setValue(0);
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObjectField *field = obj->getField(QString("measure_var")); InsSettings::DataFields insSettingsData = insSettings->getData();
field->setValue("MEASURE"); algorithm = insSettingsData.Algorithm;
obj->updated(); insSettingsData.Algorithm = InsSettings::ALGORITHM_CALIBRATION;
insSettings->setData(insSettingsData);
insSettings->updated();
collectingData = true;
QTimer::singleShot(calibrationDelay*1000, this, SLOT(calibPhase2())); connect(insSettings,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(noiseMeasured()));
m_ahrs->calibProgress->setRange(0,calibrationDelay); m_ahrs->calibProgress->setRange(0,calibrationDelay*10);
phaseCounter = 0; progressBarTimer.start(100);
progressBarIndex = 0;
connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress()));
progressBarTimer.start(1000);
} }
/** /**
Increment progress bar Increment progress bar for noise measurements (not really based on feedback)
*/ */
void ConfigAHRSWidget::incrementProgress() void ConfigAHRSWidget::incrementProgress()
{ {
m_ahrs->calibProgress->setValue(progressBarIndex++); m_ahrs->calibProgress->setValue(m_ahrs->calibProgress->value()+1);
if (progressBarIndex > m_ahrs->calibProgress->maximum()) { if (m_ahrs->calibProgress->value() >= m_ahrs->calibProgress->maximum()) {
progressBarTimer.stop(); progressBarTimer.stop();
progressBarIndex = 0;
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
collectingData = false;
QErrorMessage err(this);
err.showMessage("Noise measurement timed out. State undetermined. Please power cycle.");
err.exec();
} }
} }
/** /**
Callback once calibration is done on the board. *@brief Callback once calibration is done on the board. Restores the original algorithm.
Currently we don't have a way to tell if calibration is finished, so we
have to use a timer.
calibPhase2 is also connected to the AHRSCalibration object update signal.
*/ */
void ConfigAHRSWidget::calibPhase2() void ConfigAHRSWidget::noiseMeasured()
{ {
Q_ASSERT(collectingData); // Let's catch any race conditions
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); // Do all the clean stopping stuff
// UAVObjectField *field = obj->getField(QString("measure_var")); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
collectingData = false;
progressBarTimer.stop();
m_ahrs->calibProgress->setValue(m_ahrs->calibProgress->maximum());
// This is a bit weird, but it is because we are expecting an update from the InsSettings::DataFields insSettingsData = insSettings->getData();
// OP board with the correct calibration values, and those only arrive on the object update insSettingsData.Algorithm = algorithm;
// which comes back from the board, and not the first object update signal which is in fast insSettings->setData(insSettingsData);
// the object update we did ourselves... Clear ? insSettings->updated();
switch (phaseCounter) {
case 0: m_ahrs->calibInstructions->setText(QString("Calibration complete."));
phaseCounter++;
m_ahrs->calibInstructions->setText("Getting results...");
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2()));
// We need to echo back the results of calibration before changing to set mode
obj->requestUpdate();
break;
case 1: // This is the update with the right values (coming from the board)
disconnect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2()));
// Now update size of all the graphs
drawVariancesGraph();
saveAHRSCalibration();
m_ahrs->calibInstructions->setText(QString("Calibration saved."));
m_ahrs->ahrsCalibStart->setEnabled(true); m_ahrs->ahrsCalibStart->setEnabled(true);
break;
}
} }
/** /**
@ -573,11 +572,8 @@ void ConfigAHRSWidget::calibPhase2()
*/ */
void ConfigAHRSWidget::saveAHRSCalibration() void ConfigAHRSWidget::saveAHRSCalibration()
{ {
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObjectField *field = obj->getField(QString("measure_var")); saveObjectToSD(insSettings);
field->setValue("SET");
obj->updated();
saveObjectToSD(obj);
} }
FORCE_ALIGN_FUNC FORCE_ALIGN_FUNC

8
ground/openpilotgcs/src/plugins/config/configahrswidget.h
View File

@ -40,6 +40,7 @@
#include <QList> #include <QList>
#include <QTimer> #include <QTimer>
#include <QMutex> #include <QMutex>
#include <inssettings.h>
#include <Eigen/Core> #include <Eigen/Core>
@ -75,7 +76,7 @@ private:
int progressBarIndex; int progressBarIndex;
QTimer progressBarTimer; QTimer progressBarTimer;
const static double maxVarValue; const static double maxVarValue;
const static int calibrationDelay; const static int calibrationDelay = 10;
bool collectingData; bool collectingData;
@ -88,6 +89,7 @@ private:
QList<double> mag_accum_x; QList<double> mag_accum_x;
QList<double> mag_accum_y; QList<double> mag_accum_y;
QList<double> mag_accum_z; QList<double> mag_accum_z;
quint8 algorithm;
// TODO: Store these in std::vectors // TODO: Store these in std::vectors
Eigen::Vector3f gyro_data[60]; Eigen::Vector3f gyro_data[60];
@ -125,11 +127,11 @@ private:
private slots: private slots:
void enableHomeLocSave(UAVObject *obj); void enableHomeLocSave(UAVObject *obj);
void launchAHRSCalibration(); void measureNoise();
void noiseMeasured();
void saveAHRSCalibration(); void saveAHRSCalibration();
void openHelp(); void openHelp();
void launchAccelBiasCalibration(); void launchAccelBiasCalibration();
void calibPhase2();
void incrementProgress(); void incrementProgress();
virtual void refreshValues(); virtual void refreshValues();