mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-01 18:29:16 +01:00
Get the sensor noise measurement working again, but through GCS now
This commit is contained in:
parent
ce487c9b0a
commit
03705771dd
@ -217,6 +217,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
|
||||
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()));
|
||||
@ -327,8 +328,8 @@ void ConfigRevoWidget::doGetAccelBiasData(UAVObject *obj)
|
||||
*/
|
||||
void ConfigRevoWidget::incrementProgress()
|
||||
{
|
||||
m_ui->calibProgress->setValue(m_ui->calibProgress->value()+1);
|
||||
if (m_ui->calibProgress->value() >= m_ui->calibProgress->maximum()) {
|
||||
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());
|
||||
@ -751,16 +752,139 @@ void ConfigRevoWidget::drawVariancesGraph()
|
||||
*/
|
||||
void ConfigRevoWidget::doStartNoiseMeasurement()
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
Q_UNUSED(lock);
|
||||
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
gyro_accum_x.clear();
|
||||
gyro_accum_y.clear();
|
||||
gyro_accum_z.clear();
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
/* Need to get as many accel, mag and gyro updates as possible */
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
Gyros * gyros = Gyros::GetInstance(getObjectManager());
|
||||
Q_ASSERT(gyros);
|
||||
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
|
||||
UAVObject::Metadata mdata;
|
||||
|
||||
initialAccelsMdata = accels->getMetadata();
|
||||
mdata = initialAccelsMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
accels->setMetadata(mdata);
|
||||
|
||||
initialGyrosMdata = gyros->getMetadata();
|
||||
mdata = initialGyrosMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
gyros->setMetadata(mdata);
|
||||
|
||||
initialMagMdata = mag->getMetadata();
|
||||
mdata = initialMagMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
mag->setMetadata(mdata);
|
||||
|
||||
/* Connect for updates */
|
||||
connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
connect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
connect(mag, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when any of the sensors are updated. Stores the sample for measuring the
|
||||
* variance at the end
|
||||
*/
|
||||
void ConfigRevoWidget::doGetNoiseSample(UAVObject *)
|
||||
void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
Q_UNUSED(lock);
|
||||
|
||||
Q_ASSERT(obj);
|
||||
|
||||
switch(obj->getObjID()) {
|
||||
case Gyros::OBJID:
|
||||
{
|
||||
Gyros * gyros = Gyros::GetInstance(getObjectManager());
|
||||
Q_ASSERT(gyros);
|
||||
Gyros::DataFields gyroData = gyros->getData();
|
||||
gyro_accum_x.append(gyroData.x);
|
||||
gyro_accum_y.append(gyroData.y);
|
||||
gyro_accum_z.append(gyroData.z);
|
||||
break;
|
||||
}
|
||||
case Accels::OBJID:
|
||||
{
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
Accels::DataFields accelsData = accels->getData();
|
||||
accel_accum_x.append(accelsData.x);
|
||||
accel_accum_y.append(accelsData.y);
|
||||
accel_accum_z.append(accelsData.z);
|
||||
break;
|
||||
}
|
||||
case Magnetometer::OBJID:
|
||||
{
|
||||
Magnetometer * mags = Magnetometer::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mags);
|
||||
Magnetometer::DataFields magData = mags->getData();
|
||||
mag_accum_x.append(magData.x);
|
||||
mag_accum_y.append(magData.y);
|
||||
mag_accum_z.append(magData.z);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
Q_ASSERT(0);
|
||||
}
|
||||
|
||||
float p1 = (float) mag_accum_x.length() / (float) NOISE_SAMPLES;
|
||||
float p2 = (float) gyro_accum_x.length() / (float) NOISE_SAMPLES;
|
||||
float p3 = (float) accel_accum_x.length() / (float) NOISE_SAMPLES;
|
||||
|
||||
float prog = (p1 < p2) ? p1 : p2;
|
||||
prog = (prog < p3) ? prog : p3;
|
||||
|
||||
m_ui->noiseMeasurementProgress->setValue(prog * 100);
|
||||
|
||||
if(mag_accum_x.length() >= NOISE_SAMPLES &&
|
||||
gyro_accum_x.length() >= NOISE_SAMPLES &&
|
||||
accel_accum_x.length() >= NOISE_SAMPLES) {
|
||||
|
||||
// No need to for more updates
|
||||
Magnetometer * mags = Magnetometer::GetInstance(getObjectManager());
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Gyros * gyros = Gyros::GetInstance(getObjectManager());
|
||||
disconnect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
if(revoCalibration) {
|
||||
RevoCalibration::DataFields revoCalData = revoCalibration->getData();
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_BIAS_X] = listVar(accel_accum_x);
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_BIAS_Y] = listVar(accel_accum_y);
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_BIAS_Z] = listVar(accel_accum_z);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_BIAS_X] = listVar(gyro_accum_x);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_BIAS_Y] = listVar(gyro_accum_y);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_BIAS_Z] = listVar(gyro_accum_z);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_BIAS_X] = listVar(mag_accum_x);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_BIAS_Y] = listVar(mag_accum_y);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_BIAS_Z] = listVar(mag_accum_z);
|
||||
revoCalibration->setData(revoCalData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -771,7 +895,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *)
|
||||
{
|
||||
drawVariancesGraph();
|
||||
|
||||
m_ui->ahrsCalibStart->setEnabled(true);
|
||||
m_ui->noiseMeasurementStart->setEnabled(true);
|
||||
m_ui->sixPointsStart->setEnabled(true);
|
||||
m_ui->accelBiasStart->setEnabled(true);
|
||||
m_ui->startDriftCalib->setEnabled(true);
|
||||
|
@ -97,6 +97,8 @@ private:
|
||||
|
||||
int position;
|
||||
|
||||
static const int NOISE_SAMPLES = 100;
|
||||
|
||||
private slots:
|
||||
void launchAccelBiasCalibration();
|
||||
void incrementProgress();
|
||||
@ -108,6 +110,10 @@ private slots:
|
||||
void doGetSixPointCalibrationMeasurement(UAVObject * obj);
|
||||
void doGetAccelBiasData(UAVObject*);
|
||||
|
||||
// Slots for measuring the sensor noise
|
||||
void doStartNoiseMeasurement();
|
||||
void doGetNoiseSample(UAVObject *);
|
||||
|
||||
protected:
|
||||
void showEvent(QShowEvent *event);
|
||||
void resizeEvent(QResizeEvent *event);
|
||||
|
@ -194,7 +194,7 @@ Hint: run this with engines at cruising speed.</string>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>15</number>
|
||||
<number>100</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
|
@ -44,7 +44,6 @@ PathCompiler::PathCompiler(QObject *parent) :
|
||||
/* Connect the object updates */
|
||||
int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID);
|
||||
for (int i = 0; i < numWaypoints; i++) {
|
||||
qDebug() << "Registering waypoint" << i;
|
||||
Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i);
|
||||
Q_ASSERT(waypoint);
|
||||
if(waypoint)
|
||||
|
@ -201,6 +201,7 @@ UAVObjectManager* ConfigTaskWidget::getObjectManager() {
|
||||
Q_ASSERT(objMngr);
|
||||
return objMngr;
|
||||
}
|
||||
|
||||
/**
|
||||
* Utility function which calculates the Mean value of a list of values
|
||||
* @param list list of double values
|
||||
@ -214,6 +215,28 @@ double ConfigTaskWidget::listMean(QList<double> list)
|
||||
return accum / list.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* Utility function which calculates the Variance value of a list of values
|
||||
* @param list list of double values
|
||||
* @returns Variance of the list of parameter values
|
||||
*/
|
||||
double ConfigTaskWidget::listVar(QList<double> list)
|
||||
{
|
||||
double mean_accum = 0;
|
||||
double var_accum = 0;
|
||||
double mean;
|
||||
|
||||
for(int i = 0; i < list.size(); i++)
|
||||
mean_accum += list[i];
|
||||
mean = mean_accum / list.size();
|
||||
|
||||
for(int i = 0; i < list.size(); i++)
|
||||
var_accum += (list[i] - mean) * (list[i] - mean);
|
||||
|
||||
// Use unbiased estimator
|
||||
return var_accum / (list.size() - 1);
|
||||
}
|
||||
|
||||
// ************************************
|
||||
// telemetry start/stop connect/disconnect signals
|
||||
|
||||
|
@ -92,6 +92,7 @@ public:
|
||||
void saveObjectToSD(UAVObject *obj);
|
||||
UAVObjectManager* getObjectManager();
|
||||
static double listMean(QList<double> list);
|
||||
static double listVar(QList<double> list);
|
||||
|
||||
void addUAVObject(QString objectName);
|
||||
void addWidget(QWidget * widget);
|
||||
|
Loading…
x
Reference in New Issue
Block a user