1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00

OP-716 Added checkbox to enable accelerator filtering (cyr fix)

This commit is contained in:
Fredrik Arvidsson 2012-11-16 21:22:13 +01:00
parent 22173d96e5
commit 95455e2378
3 changed files with 54 additions and 10 deletions

View File

@ -111,7 +111,7 @@
<x>0</x>
<y>0</y>
<width>750</width>
<height>466</height>
<height>483</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
@ -367,16 +367,27 @@ margin:1px;</string>
</spacer>
</item>
<item>
<widget class="QCheckBox" name="zeroGyroBiasOnArming">
<property name="toolTip">
<string>If enabled, a fast recalibration of gyro zero point will be done
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QCheckBox" name="zeroGyroBiasOnArming">
<property name="toolTip">
<string>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!</string>
</property>
<property name="text">
<string>Zero gyros while arming aircraft</string>
</property>
</widget>
</property>
<property name="text">
<string>Zero gyros while arming aircraft</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="accelFiltering">
<property name="text">
<string>Accel filtering</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_6">

View File

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

View File

@ -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<double> x_accum, y_accum, z_accum;
QList<double> 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