diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui
index afe6965a2..91c89448f 100755
--- a/ground/openpilotgcs/src/plugins/config/ccattitude.ui
+++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui
@@ -111,7 +111,7 @@
0
0
750
- 466
+ 483
@@ -391,6 +391,86 @@ arming it in that case!
+ -
+
+
-
+
+
+
+ 0
+ 0
+
+
+
+
+
+
+ AccelTau
+
+
+
+ -
+
+
+ Qt::Horizontal
+
+
+ QSizePolicy::Fixed
+
+
+
+ 10
+ 0
+
+
+
+
+ -
+
+
+
+ 60
+ 0
+
+
+
+ Accelerometer filtering.
+
+Sets the amount of lowpass filtering of accelerometer data
+for the attitude estimation. Higher values apply a stronger
+filter, which may help with drifting in attitude mode.
+
+Range: 0.00 - 0.20, Good starting value: 0.05 - 0.10
+Start low and raise until drift stops.
+
+A setting of 0.00 disables the filter.
+
+
+ 2
+
+
+ 0.200000000000000
+
+
+ 0.010000000000000
+
+
+
+
+
+ -
+
+
+ Qt::Horizontal
+
+
+
+ 40
+ 20
+
+
+
+
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
index fd800811c..febfcf4e6 100644
--- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
@@ -56,7 +56,9 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
// Connect the help button
connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
+
addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming);
+ addUAVObjectToWidgetRelation("AttitudeSettings", "AccelTau", ui->accelTauSpinbox);
addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->rollBias,AttitudeSettings::BOARDROTATION_ROLL);
addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->pitchBias,AttitudeSettings::BOARDROTATION_PITCH);
@@ -73,8 +75,8 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget()
void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) {
if (!timer.isActive()) {
- // ignore updates that come in after the timer has expired
- return;
+ // ignore updates that come in after the timer has expired
+ return;
}
Accels * accels = Accels::GetInstance(getObjectManager());
@@ -207,16 +209,27 @@ void ConfigCCAttitudeWidget::openHelp()
QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) );
}
+void ConfigCCAttitudeWidget::setAccelFiltering(bool active)
+{
+ setDirty(true);
+}
+
void ConfigCCAttitudeWidget::enableControls(bool enable)
{
- if(ui->zeroBias)
+ if(ui->zeroBias) {
ui->zeroBias->setEnabled(enable);
+ }
+ if(ui->zeroGyroBiasOnArming) {
+ ui->zeroGyroBiasOnArming->setEnabled(enable);
+ }
+ if(ui->accelTauSpinbox) {
+ ui->accelTauSpinbox->setEnabled(enable);
+ }
ConfigTaskWidget::enableControls(enable);
}
void ConfigCCAttitudeWidget::updateObjectsFromWidgets()
{
ConfigTaskWidget::updateObjectsFromWidgets();
-
ui->zeroBiasProgress->setValue(0);
}
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
index 0637d268a..b171035d1 100644
--- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
@@ -52,6 +52,7 @@ private slots:
void timeout();
void startAccelCalibration();
void openHelp();
+ void setAccelFiltering(bool active);
private:
Ui_ccattitude *ui;
@@ -65,6 +66,7 @@ private:
QList x_accum, y_accum, z_accum;
QList x_gyro_accum, y_gyro_accum, z_gyro_accum;
+ static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1;
static const int NUM_SENSOR_UPDATES = 300;
static const float ACCEL_SCALE = 0.004f * 9.81f;
protected:
diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
index 2c86dc2e1..d62fdd1e3 100644
--- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
+++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
@@ -292,12 +292,12 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration()
void VehicleConfigurationHelper::applyLevellingConfiguration()
{
+ AttitudeSettings* attitudeSettings = AttitudeSettings::GetInstance(m_uavoManager);
+ Q_ASSERT(attitudeSettings);
+ AttitudeSettings::DataFields data = attitudeSettings->getData();
if(m_configSource->isLevellingPerformed())
{
accelGyroBias bias = m_configSource->getLevellingBias();
- AttitudeSettings* attitudeSettings = AttitudeSettings::GetInstance(m_uavoManager);
- Q_ASSERT(attitudeSettings);
- AttitudeSettings::DataFields data = attitudeSettings->getData();
data.AccelBias[0] += bias.m_accelerometerXBias;
data.AccelBias[1] += bias.m_accelerometerYBias;
@@ -305,10 +305,10 @@ void VehicleConfigurationHelper::applyLevellingConfiguration()
data.GyroBias[0] = -bias.m_gyroXBias;
data.GyroBias[1] = -bias.m_gyroYBias;
data.GyroBias[2] = -bias.m_gyroZBias;
-
- attitudeSettings->setData(data);
- addModifiedObject(attitudeSettings, tr("Writing gyro and accelerometer bias settings"));
}
+ data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU;
+ attitudeSettings->setData(data);
+ addModifiedObject(attitudeSettings, tr("Writing gyro and accelerometer bias settings"));
}
void VehicleConfigurationHelper::applyStabilizationConfiguration()
diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h
index 44f2125b1..b72e40f63 100644
--- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h
+++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h
@@ -68,6 +68,7 @@ private:
static const int MIXER_TYPE_DISABLED = 0;
static const int MIXER_TYPE_MOTOR = 1;
static const int MIXER_TYPE_SERVO = 2;
+ static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1;
VehicleConfigurationSource *m_configSource;
UAVObjectManager *m_uavoManager;