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

Don't restore the accel metadata after six point calibration unless it was

Browse Source 
actually saved and changed
This commit is contained in:
James Cotton 2012-06-14 09:57:46 -05:00
parent 4f37be785e
commit a91c21f53f
1 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -415,19 +415,22 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject * obj)
#endif #endif
collectingData = false; collectingData = false;
m_ui->sixPointsSave->setEnabled(true);
#ifdef SIX_POINT_CAL_ACCEL
// Store the mean for this position for the accel
Accels * accels = Accels::GetInstance(getObjectManager()); Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels); Q_ASSERT(accels);
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*))); disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
disconnect(mag,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
m_ui->sixPointsSave->setEnabled(true);
#ifdef SIX_POINT_CAL_ACCEL
accel_data_x[position] = listMean(accel_accum_x); accel_data_x[position] = listMean(accel_accum_x);
accel_data_y[position] = listMean(accel_accum_y); accel_data_y[position] = listMean(accel_accum_y);
accel_data_z[position] = listMean(accel_accum_z); accel_data_z[position] = listMean(accel_accum_z);
#endif #endif
// Store the mean for this position for the mag
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(mag,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
mag_data_x[position] = listMean(mag_accum_x); mag_data_x[position] = listMean(mag_accum_x);
mag_data_y[position] = listMean(mag_accum_y); mag_data_y[position] = listMean(mag_accum_y);
mag_data_z[position] = listMean(mag_accum_z); mag_data_z[position] = listMean(mag_accum_z);
@ -459,7 +462,9 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject * obj)
m_ui->sixPointsSave->setEnabled(false); m_ui->sixPointsSave->setEnabled(false);
/* Cleanup original settings */ /* Cleanup original settings */
#ifdef SIX_POINT_CAL_ACCEL
accels->setMetadata(initialAccelsMdata); accels->setMetadata(initialAccelsMdata);
#endif
mag->setMetadata(initialMagMdata); mag->setMetadata(initialMagMdata);
} }
} }
@ -597,10 +602,14 @@ int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z
return 1; return 1;
} }
/**
* Computes the scale and bias for the magnetomer and (compile option)
* for the accel once all the data has been collected in 6 positions.
*/
void ConfigRevoWidget::computeScaleBias() void ConfigRevoWidget::computeScaleBias()
{ {
double S[3], b[3]; double S[3], b[3];
double Be_lenght; double Be_length;
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager()); HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
@ -621,8 +630,8 @@ void ConfigRevoWidget::computeScaleBias()
#endif #endif
// Calibration mag // Calibration mag
Be_lenght = sqrt(pow(homeLocationData.Be[0],2)+pow(homeLocationData.Be[1],2)+pow(homeLocationData.Be[2],2)); Be_length = sqrt(pow(homeLocationData.Be[0],2)+pow(homeLocationData.Be[1],2)+pow(homeLocationData.Be[2],2));
SixPointInConstFieldCal( Be_lenght, mag_data_x, mag_data_y, mag_data_z, S, b); SixPointInConstFieldCal( Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]);
@ -699,18 +708,21 @@ void ConfigRevoWidget::doStartSixPointCalibration()
mag_accum_y.clear(); mag_accum_y.clear();
mag_accum_z.clear(); mag_accum_z.clear();
/* Need to get as many accel and mag updates as possible */ #ifdef SIX_POINT_CAL_ACCEL
/* Need to get as many accel updates as possible */
Accels * accels = Accels::GetInstance(getObjectManager()); Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels); Q_ASSERT(accels);
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
initialAccelsMdata = accels->getMetadata(); initialAccelsMdata = accels->getMetadata();
UAVObject::Metadata mdata = initialAccelsMdata; UAVObject::Metadata mdata = initialAccelsMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100; mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata); accels->setMetadata(mdata);
#endif
/* Need to get as many mag updates as possible */
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
initialMagMdata = mag->getMetadata(); initialMagMdata = mag->getMetadata();
mdata = initialMagMdata; mdata = initialMagMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);