diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui
index afe6965a2..445a05d37 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
@@ -367,16 +367,27 @@ margin:1px;
-
-
-
- If enabled, a fast recalibration of gyro zero point will be done
+
+
-
+
+
+ If enabled, a fast recalibration of gyro zero point will be done
whenever the frame is armed. Do not move the airframe while
arming it in that case!
-
-
- Zero gyros while arming aircraft
-
-
+
+
+ Zero gyros while arming aircraft
+
+
+
+ -
+
+
+ Accel filtering
+
+
+
+
-
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
index fd800811c..48f4768d7 100644
--- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
@@ -56,6 +56,8 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
// Connect the help button
connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
+
+ connect(ui->accelFiltering, SIGNAL(toggled(bool)), this, SLOT(setAccelFiltering(bool)));
addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming);
addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->rollBias,AttitudeSettings::BOARDROTATION_ROLL);
@@ -207,13 +209,39 @@ void ConfigCCAttitudeWidget::openHelp()
QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/44Cf", QUrl::StrictMode) );
}
+void ConfigCCAttitudeWidget::setAccelFiltering(bool active)
+{
+ AttitudeSettings* settings = AttitudeSettings::GetInstance(getObjectManager());
+ Q_ASSERT(settings);
+ AttitudeSettings::DataFields data = settings->getData();
+
+ data.AccelTau = active ? DEFAULT_ENABLED_ACCEL_TAU : 0.0f;
+
+ settings->setData(data);
+ setDirty(true);
+}
+
void ConfigCCAttitudeWidget::enableControls(bool enable)
{
- if(ui->zeroBias)
+ if(ui->zeroBias) {
ui->zeroBias->setEnabled(enable);
+ }
+ if(ui->accelFiltering) {
+ ui->accelFiltering->setEnabled(enable);
+ }
ConfigTaskWidget::enableControls(enable);
}
+void ConfigCCAttitudeWidget::refreshWidgetsValues(UAVObject *obj)
+{
+ AttitudeSettings* settings = AttitudeSettings::GetInstance(getObjectManager());
+ Q_ASSERT(settings);
+ AttitudeSettings::DataFields data = settings->getData();
+ ui->accelFiltering->setChecked(data.AccelTau > 0.0f);
+
+ ConfigTaskWidget::refreshWidgetsValues(obj);
+}
+
void ConfigCCAttitudeWidget::updateObjectsFromWidgets()
{
ConfigTaskWidget::updateObjectsFromWidgets();
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
index 0637d268a..7f1eabfe4 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,11 +66,15 @@ 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.15;
static const int NUM_SENSOR_UPDATES = 300;
static const float ACCEL_SCALE = 0.004f * 9.81f;
protected:
virtual void enableControls(bool enable);
+protected slots:
+ void refreshWidgetsValues(UAVObject * obj=NULL);
+
};
#endif // CCATTITUDEWIDGET_H