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

Get the sensor noise measurement working again, but through GCS now

Browse Source
This commit is contained in:
James Cotton 2012-06-13 21:26:24 -05:00
parent ce487c9b0a
commit 03705771dd
6 changed files with 159 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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);

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

@ -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);

2
ground/openpilotgcs/src/plugins/config/revosensors.ui
View File

@ -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>

1
ground/openpilotgcs/src/plugins/opmap/pathcompiler.cpp
View File

@ -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)

23
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp
View File

@ -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

1
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h
View File

@ -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);