From c5745b08173df318778fde6ae72ac24529076993 Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Sat, 8 Sep 2012 15:58:39 +0930 Subject: [PATCH 1/2] Fixes/tweaks to GCS levelling code. Fixed a deadlock and take more samples --- .../plugins/config/configccattitudewidget.cpp | 27 ++++++++++--------- .../plugins/config/configccattitudewidget.h | 4 +-- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index fe1f517bf..11beeaff3 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -64,8 +64,13 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() } void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { - QMutexLocker locker(&startStop); + if (!timer.isActive()) { + // ignore updates that come in after the timer has expired + return; + } + + // update the progress indicator ui->zeroBiasProgress->setValue((float) qMin(accelUpdates,gyroUpdates) / NUM_SENSOR_UPDATES * 100); Accels * accels = Accels::GetInstance(getObjectManager()); @@ -83,7 +88,7 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } else if ( accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { + } else if ( accelUpdates >= NUM_SENSOR_UPDATES || gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -115,7 +120,6 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { } void ConfigCCAttitudeWidget::timeout() { - QMutexLocker locker(&startStop); UAVDataObject * obj = Accels::GetInstance(getObjectManager()); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -130,12 +134,11 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - + // reset progress indicator + ui->zeroBiasProgress->setValue(0); } void ConfigCCAttitudeWidget::startAccelCalibration() { - QMutexLocker locker(&startStop); - accelUpdates = 0; gyroUpdates = 0; x_accum.clear(); @@ -156,10 +159,6 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { connect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); connect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - // Set up timeout timer - timer.start(10000); - connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); - // Speed up updates initialAccelsMdata = accels->getMetadata(); UAVObject::Metadata accelsMdata = initialAccelsMdata; @@ -173,11 +172,16 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { gyrosMdata.flightTelemetryUpdatePeriod = 30; gyros->setMetadata(gyrosMdata); + // Set up timeout timer + timer.setSingleShot(true); + timer.start(5000 + (NUM_SENSOR_UPDATES * qMin(accelsMdata.flightTelemetryUpdatePeriod, + gyrosMdata.flightTelemetryUpdatePeriod))); + connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + } void ConfigCCAttitudeWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/CopterControl+Attitude+Configuration", QUrl::StrictMode) ); } @@ -186,7 +190,6 @@ void ConfigCCAttitudeWidget::enableControls(bool enable) if(ui->zeroBias) ui->zeroBias->setEnabled(enable); ConfigTaskWidget::enableControls(enable); - } void ConfigCCAttitudeWidget::updateObjectsFromWidgets() diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index fc1a7f623..0637d268a 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -34,7 +34,6 @@ #include "uavobject.h" #include #include -#include class Ui_Widget; @@ -55,7 +54,6 @@ private slots: void openHelp(); private: - QMutex startStop; Ui_ccattitude *ui; QTimer timer; UAVObject::Metadata initialAccelsMdata; @@ -67,7 +65,7 @@ private: QList x_accum, y_accum, z_accum; QList x_gyro_accum, y_gyro_accum, z_gyro_accum; - static const int NUM_SENSOR_UPDATES = 60; + static const int NUM_SENSOR_UPDATES = 300; static const float ACCEL_SCALE = 0.004f * 9.81f; protected: virtual void enableControls(bool enable); From 5b7fb6bcef32ccdecbf3b4823f67e3728998b073 Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Mon, 10 Sep 2012 22:22:36 +0930 Subject: [PATCH 2/2] Minor improvement to GCS bias calibration logic, plus minor UI behaviour tweak --- .../plugins/config/configccattitudewidget.cpp | 42 ++++++++++++------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 11beeaff3..2131f58ea 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -43,7 +43,6 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : ui->setupUi(this); connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); - addApplySaveButtons(ui->applyButton,ui->saveButton); addUAVObject("AttitudeSettings"); @@ -70,25 +69,33 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { return; } - // update the progress indicator - ui->zeroBiasProgress->setValue((float) qMin(accelUpdates,gyroUpdates) / NUM_SENSOR_UPDATES * 100); - Accels * accels = Accels::GetInstance(getObjectManager()); Gyros * gyros = Gyros::GetInstance(getObjectManager()); - if(obj->getObjID() == Accels::OBJID && accelUpdates < NUM_SENSOR_UPDATES) { + // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples + // for both gyros and accels. + // Note that, at present, we stash the samples and then compute the bias + // at the end, even though the mean could be accumulated as we go. + // In future, a better algorithm could be used. + if(obj->getObjID() == Accels::OBJID) { accelUpdates++; Accels::DataFields accelsData = accels->getData(); x_accum.append(accelsData.x); y_accum.append(accelsData.y); z_accum.append(accelsData.z); - } else if (obj->getObjID() == Gyros::OBJID && gyroUpdates < NUM_SENSOR_UPDATES) { + } else if (obj->getObjID() == Gyros::OBJID) { gyroUpdates++; Gyros::DataFields gyrosData = gyros->getData(); x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } else if ( accelUpdates >= NUM_SENSOR_UPDATES || gyroUpdates >= NUM_SENSOR_UPDATES) { + } + + // update the progress indicator + ui->zeroBiasProgress->setValue((float) qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + + // If we have enough samples, then stop sampling and compute the biases + if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -113,9 +120,9 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { attitudeSettingsData.GyroBias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); - } else { - // Possible to get here if weird threading stuff happens. Just ignore updates. - qDebug("Unexpected accel update received."); + + // reenable controls + enableControls(true); } } @@ -134,11 +141,17 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); + // reset progress indicator - ui->zeroBiasProgress->setValue(0); + ui->zeroBiasProgress->setValue(0); + // reenable controls + enableControls(true); } void ConfigCCAttitudeWidget::startAccelCalibration() { + // disable controls during sampling + enableControls(false); + accelUpdates = 0; gyroUpdates = 0; x_accum.clear(); @@ -163,21 +176,20 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { initialAccelsMdata = accels->getMetadata(); UAVObject::Metadata accelsMdata = initialAccelsMdata; UAVObject::SetFlightTelemetryUpdateMode(accelsMdata, UAVObject::UPDATEMODE_PERIODIC); - accelsMdata.flightTelemetryUpdatePeriod = 30; + accelsMdata.flightTelemetryUpdatePeriod = 30; // ms accels->setMetadata(accelsMdata); initialGyrosMdata = gyros->getMetadata(); UAVObject::Metadata gyrosMdata = initialGyrosMdata; UAVObject::SetFlightTelemetryUpdateMode(gyrosMdata, UAVObject::UPDATEMODE_PERIODIC); - gyrosMdata.flightTelemetryUpdatePeriod = 30; + gyrosMdata.flightTelemetryUpdatePeriod = 30; // ms gyros->setMetadata(gyrosMdata); // Set up timeout timer timer.setSingleShot(true); - timer.start(5000 + (NUM_SENSOR_UPDATES * qMin(accelsMdata.flightTelemetryUpdatePeriod, + timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, gyrosMdata.flightTelemetryUpdatePeriod))); connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); - } void ConfigCCAttitudeWidget::openHelp()