From 4797aa8b19557ca7b5803f01dbcd8e85f6a19f85 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sat, 6 Sep 2014 18:51:59 +0200 Subject: [PATCH 01/21] OP-1474 Added fields for thrust pid scaling. --- .../stabilizationsettings.xml | 79 ++++++++++--------- 1 file changed, 41 insertions(+), 38 deletions(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index a79798b4c..5f42cdfa6 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -3,53 +3,56 @@ PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - + + - - - - - - - - + + + + + + + + - - - + + + - - - + + + - - + + - + - - - - - - - + + + + + + + - - + + - + - - + + - - - - + + + + + + + From 7fb064cf22549504c0f60594e06f85f1baeeb3eb Mon Sep 17 00:00:00 2001 From: m_thread Date: Sat, 6 Sep 2014 23:53:44 +0200 Subject: [PATCH 02/21] OP-1474 Added thrust PID scale curve to stabilization configuration GUI. --- .../config/configstabilizationwidget.cpp | 58 +++ .../config/configstabilizationwidget.h | 5 + .../src/plugins/config/mixercurve.cpp | 11 + .../src/plugins/config/mixercurve.h | 2 +- .../src/plugins/config/mixercurve.ui | 193 ++++---- .../src/plugins/config/stabilization.ui | 416 +++++++++++------- .../uavobjectwidgetutils/mixercurvewidget.cpp | 4 +- 7 files changed, 433 insertions(+), 256 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 951f94550..f154aca4c 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -123,6 +123,16 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa addWidget(ui->advancedResponsivenessCheckBox); connect(ui->advancedResponsivenessCheckBox, SIGNAL(toggled(bool)), this, SLOT(linkCheckBoxes(bool))); + connect(ui->defaultThrottleCurveButton, SIGNAL(clicked()), this, SLOT(resetThrottleCurveToDefault())); + connect(ui->thrustPIDScalingGroup, SIGNAL(toggled(bool)), ui->thrustPIDScalingCurve, SLOT(setEnabled(bool))); + ui->thrustPIDScalingCurve->setMixerType(MixerCurve::MIXERCURVE_TPA); + ui->thrustPIDScalingCurve->setMin(-1); + ui->thrustPIDScalingCurve->setMax(1); + + addWidget(ui->thrustPIDScalingGroup); + addWidget(ui->thrustPIDScalingCurve); + addWidget(ui->thrustPIDScalingCurve->getCurveWidget()); + connect(this, SIGNAL(widgetContentsChanged(QWidget *)), this, SLOT(processLinkedWidgets(QWidget *))); connect(this, SIGNAL(autoPilotConnected()), this, SLOT(onBoardConnected())); @@ -140,10 +150,58 @@ void ConfigStabilizationWidget::refreshWidgetsValues(UAVObject *o) { ConfigTaskWidget::refreshWidgetsValues(o); + updateThrottleCurveFromObject(); + ui->basicResponsivenessCheckBox->setChecked(ui->rateRollKp_3->value() == ui->ratePitchKp_4->value() && ui->rateRollKi_3->value() == ui->ratePitchKi_4->value()); } +void ConfigStabilizationWidget::updateObjectsFromWidgets() +{ + updateObjectFromThrottleCurve(); + ConfigTaskWidget::updateObjectsFromWidgets(); +} + +void ConfigStabilizationWidget::updateThrottleCurveFromObject() +{ + StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + Q_ASSERT(stabSettings); + + QList curve; + for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { + curve.append(stabSettings->getThrustPIDScaleCurve(i)); + } + + ui->thrustPIDScalingCurve->setCurve(&curve); + ui->thrustPIDScalingGroup->setChecked(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); + ui->thrustPIDScalingCurve->setEnabled(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); +} + +void ConfigStabilizationWidget::updateObjectFromThrottleCurve() +{ + StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + Q_ASSERT(stabSettings); + + QList curve = ui->thrustPIDScalingCurve->getCurve(); + for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { + stabSettings->setThrustPIDScaleCurve(i, curve.at(i)); + } + + stabSettings->setEnableThrustPIDScaling(ui->thrustPIDScalingGroup->isChecked() ? + StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE : StabilizationSettings::ENABLETHRUSTPIDSCALING_FALSE); +} + +void ConfigStabilizationWidget::resetThrottleCurveToDefault() +{ + StabilizationSettings defaultSettings; + QList curve; + for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { + curve.append(defaultSettings.getThrustPIDScaleCurve(i)); + } + + ui->thrustPIDScalingCurve->setCurve(&curve); +} + void ConfigStabilizationWidget::realtimeUpdatesSlot(bool value) { ui->realTimeUpdates_6->setChecked(value); diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index e4312df58..5b3c536f6 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -57,11 +57,15 @@ private: int boardModel; int m_pidBankCount; int m_currentPIDBank; + + void updateThrottleCurveFromObject(); + void updateObjectFromThrottleCurve(); protected: QString mapObjectName(const QString objectName); protected slots: void refreshWidgetsValues(UAVObject *o = NULL); + void updateObjectsFromWidgets(); private slots: void realtimeUpdatesSlot(bool value); @@ -69,6 +73,7 @@ private slots: void processLinkedWidgets(QWidget *); void onBoardConnected(); void pidBankChanged(int index); + void resetThrottleCurveToDefault(); }; #endif // ConfigStabilizationWidget_H diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.cpp b/ground/openpilotgcs/src/plugins/config/mixercurve.cpp index ad0fdaa3a..24bc0f88b 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.cpp +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.cpp @@ -75,6 +75,7 @@ void MixerCurve::setMixerType(MixerCurveType curveType) { m_curveType = curveType; + m_mixerUI->buttonGroup->show(); m_mixerUI->CurveMin->setMaximum(1.0); m_mixerUI->CurveMax->setMaximum(1.0); @@ -95,6 +96,16 @@ void MixerCurve::setMixerType(MixerCurveType curveType) m_mixerUI->CurveMax->setMinimum(-1.0); break; } + case MixerCurve::MIXERCURVE_TPA: + { + m_mixerUI->SettingsGroup->setTitle("Thrust PID Scale"); + m_mixerUI->buttonGroup->hide(); + m_curve->setRange(-1.0, 1.0); + m_mixerUI->CurveMin->setMinimum(-1.0); + m_mixerUI->CurveMax->setMinimum(-1.0); + } + default: + break; } m_spinDelegate->setRange(m_mixerUI->CurveMin->minimum(), m_mixerUI->CurveMax->maximum()); diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.h b/ground/openpilotgcs/src/plugins/config/mixercurve.h index f62bfa251..7343043c9 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.h +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.h @@ -52,7 +52,7 @@ public: /* Enumeration options for ThrottleCurves */ - typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1 } MixerCurveType; + typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1, MIXERCURVE_TPA = 2 } MixerCurveType; void setMixerType(MixerCurveType curveType); void initCurve(const QList *points); diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.ui b/ground/openpilotgcs/src/plugins/config/mixercurve.ui index 37d8925ee..bdddb1e80 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.ui +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.ui @@ -7,7 +7,7 @@ 0 0 543 - 467 + 488 @@ -38,17 +38,32 @@ MixerCurve - QFrame::StyledPanel + QFrame::NoFrame QFrame::Raised - + + + + + 0 + 0 + + + + + 7 + + + + + - 150 + 120 16777215 @@ -56,11 +71,29 @@ Throttle Curve + + 0 + + + 9 + + + 0 + + + 0 + + + + 0 + 0 + + - 100 + 65535 200 @@ -69,6 +102,12 @@ 8 + + QFrame::StyledPanel + + + QFrame::Plain + Qt::ScrollBarAlwaysOff @@ -87,6 +126,12 @@ 1 + + true + + + false + Max @@ -274,91 +319,69 @@ - - + + - - 5 - 5 + + 0 + 0 - - - 50 - 50 - - - - - 1000 - 1000 - - - - - 10 - 10 - - - - - 200 - 200 - - - - - 7 - - + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Qt::Horizontal + + + + 203 + 16 + + + + + + + + + 16777215 + 32 + + + + Advanced... + + + + + + + + 16777215 + 32 + + + + Reset + + + + - - - - 4 - - - - - Qt::Horizontal - - - - 40 - 16 - - - - - - - - - 16777215 - 32 - - - - Reset - - - - - - - - 16777215 - 32 - - - - Advanced... - - - - - diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index f2e5042dd..7a449afa9 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -136,8 +136,8 @@ 0 0 - 798 - 705 + 782 + 726 @@ -7837,8 +7837,8 @@ border-radius: 5; 0 0 - 565 - 733 + 782 + 721 @@ -16299,8 +16299,8 @@ border-radius: 5; 0 0 - 829 - 691 + 831 + 683 @@ -23904,6 +23904,9 @@ font:bold; 0 + + QAbstractScrollArea::AdjustIgnored + true @@ -23912,70 +23915,31 @@ font:bold; 0 0 - 665 - 435 + 798 + 699 + + QLayout::SetDefaultConstraint + - - - Rattitude - - - - 9 - - - 9 - - - 9 - - - 9 - - - - - Qt::Horizontal + + + + + + Thrust PID Scaling - - - 40 - 20 - - - - - - - - Default - - - - objname:StabilizationSettings - button:default - buttongroup:15 - - - - - - - - QGroupBox{border: 0px;} - - + true - + 0 - 9 + 0 0 @@ -23983,124 +23947,250 @@ font:bold; 0 - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 90 - 11 - - - - - - + + - + 0 0 - 5 - 22 + 200 + 200 - - - 175 - 22 - + + + + + + QFrame::NoFrame - - Qt::StrongFocus + + QFrame::Raised - - <html><head/><body><p>Percentage of full stick where the transition from Attitude to Rate occurs. This transition always occurs when the aircraft is exactly inverted (bank angle 180 degrees). Small values are dangerous because they cause flips at small stick angles. Values significantly over 100 act like attitude mode and can never flip.</p></body></html> + + + 9 + + + + + Default + + + + + + + + + + + + + Rattitude + + + + 9 + + + 9 + + + 9 + + + 9 + + + + + QGroupBox{border: 0px;} - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + true - - 0 - - - 25.000000000000000 - - - 255.000000000000000 - - - 80.000000000000000 + + + 0 + + + 9 + + + 0 + + + 0 + + + + + Qt::Horizontal + + + + 300 + 20 + + + + + + + + + 0 + 0 + + + + + 5 + 22 + + + + + 175 + 22 + + + + Qt::StrongFocus + + + <html><head/><body><p>Percentage of full stick where the transition from Attitude to Rate occurs. This transition always occurs when the aircraft is exactly inverted (bank angle 180 degrees). Small values are dangerous because they cause flips at small stick angles. Values significantly over 100 act like attitude mode and can never flip.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + 25.000000000000000 + + + 255.000000000000000 + + + 80.000000000000000 + + + + objname:StabilizationSettings + fieldname:RattitudeModeTransition + haslimits:no + scale:1 + buttongroup:15 + + + + + + + + + 0 + 0 + + + + + 144 + 16 + + + + + 175 + 16777215 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + ModeTransition + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 90 + 11 + + + + + + + + Qt::Vertical + + + QSizePolicy::MinimumExpanding + + + + 20 + 20 + + + + + + + + + + + Default objname:StabilizationSettings - fieldname:RattitudeModeTransition - haslimits:no - scale:1 + button:default buttongroup:15 - - - - - 0 - 0 - - - - - 144 - 16 - - - - - 175 - 16777215 - - - - - 75 - true - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; - - - ModeTransition - - - Qt::AlignCenter - - - - - + + Qt::Horizontal - 300 + 40 20 @@ -24112,22 +24202,6 @@ border-radius: 5; - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 20 - - - - @@ -24767,7 +24841,7 @@ border-radius: 5; - + Qt::Vertical @@ -24818,8 +24892,8 @@ border-radius: 5; 0 0 - 478 - 518 + 798 + 699 @@ -27843,6 +27917,12 @@ Useful if you have accidentally changed some settings.
qtabbar.h
1 + + MixerCurve + QWidget +
mixercurve.h
+ 1 + stabilizationReloadBoardData_6 diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp index 17815aca4..37d9f3be5 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp @@ -225,7 +225,7 @@ void MixerCurveWidget::showEvent(QShowEvent *event) // the result is usually a ahrsbargraph that is way too small. QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-12, -12, 12, 12), Qt::KeepAspectRatio); } void MixerCurveWidget::resizeEvent(QResizeEvent *event) @@ -233,7 +233,7 @@ void MixerCurveWidget::resizeEvent(QResizeEvent *event) Q_UNUSED(event); QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-15, -15, 15, 15), Qt::KeepAspectRatio); + fitInView(rect.adjusted(-12, -12, 12, 12), Qt::KeepAspectRatio); } void MixerCurveWidget::changeEvent(QEvent *event) From 45ea8015ed7fafd190364b300796cca709893bc4 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sun, 7 Sep 2014 00:20:22 +0200 Subject: [PATCH 03/21] OP-1474 Updated GUI to follow the standard way of handling checked groups. Added missing push buttons to widget list to make then enable/disable correctly. --- .../config/configstabilizationwidget.cpp | 11 ++- .../src/plugins/config/stabilization.ui | 89 +++++++++++-------- 2 files changed, 60 insertions(+), 40 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index f154aca4c..6daad2ced 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -112,6 +112,8 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa addWidget(ui->pushButton_5); addWidget(ui->pushButton_6); addWidget(ui->pushButton_9); + addWidget(ui->pushButton_10); + addWidget(ui->pushButton_11); addWidget(ui->pushButton_20); addWidget(ui->pushButton_22); addWidget(ui->pushButton_23); @@ -124,12 +126,13 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa connect(ui->advancedResponsivenessCheckBox, SIGNAL(toggled(bool)), this, SLOT(linkCheckBoxes(bool))); connect(ui->defaultThrottleCurveButton, SIGNAL(clicked()), this, SLOT(resetThrottleCurveToDefault())); - connect(ui->thrustPIDScalingGroup, SIGNAL(toggled(bool)), ui->thrustPIDScalingCurve, SLOT(setEnabled(bool))); + connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->thrustPIDScalingCurve, SLOT(setEnabled(bool))); ui->thrustPIDScalingCurve->setMixerType(MixerCurve::MIXERCURVE_TPA); ui->thrustPIDScalingCurve->setMin(-1); ui->thrustPIDScalingCurve->setMax(1); - addWidget(ui->thrustPIDScalingGroup); + addWidget(ui->defaultThrottleCurveButton); + addWidget(ui->enableThrustPIDScalingCheckBox); addWidget(ui->thrustPIDScalingCurve); addWidget(ui->thrustPIDScalingCurve->getCurveWidget()); @@ -173,7 +176,7 @@ void ConfigStabilizationWidget::updateThrottleCurveFromObject() } ui->thrustPIDScalingCurve->setCurve(&curve); - ui->thrustPIDScalingGroup->setChecked(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); + ui->enableThrustPIDScalingCheckBox->setChecked(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); ui->thrustPIDScalingCurve->setEnabled(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); } @@ -187,7 +190,7 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve() stabSettings->setThrustPIDScaleCurve(i, curve.at(i)); } - stabSettings->setEnableThrustPIDScaling(ui->thrustPIDScalingGroup->isChecked() ? + stabSettings->setEnableThrustPIDScaling(ui->enableThrustPIDScalingCheckBox->isChecked() ? StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE : StabilizationSettings::ENABLETHRUSTPIDSCALING_FALSE); } diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 7a449afa9..5d6a1c747 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -136,8 +136,8 @@ 0 0 - 782 - 726 + 798 + 699 @@ -23932,22 +23932,41 @@ font:bold; Thrust PID Scaling - true + false - - - 0 - - - 0 - - - 0 - - - 0 - - + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Default + + + + + + + @@ -23963,28 +23982,26 @@ font:bold; - - - - QFrame::NoFrame + + + + Enable Thrust PID Scaling - - QFrame::Raised - - - - 9 - - - - - Default - - - - + + + + Qt::Horizontal + + + + 40 + 20 + + + + From 474466ac92810665570e353fe1622db92621531a Mon Sep 17 00:00:00 2001 From: m_thread Date: Sun, 7 Sep 2014 00:23:20 +0200 Subject: [PATCH 04/21] OP-1474 Uncrustify. --- .../src/plugins/config/configstabilizationwidget.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 6daad2ced..fd01e8353 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -168,6 +168,7 @@ void ConfigStabilizationWidget::updateObjectsFromWidgets() void ConfigStabilizationWidget::updateThrottleCurveFromObject() { StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + Q_ASSERT(stabSettings); QList curve; @@ -183,6 +184,7 @@ void ConfigStabilizationWidget::updateThrottleCurveFromObject() void ConfigStabilizationWidget::updateObjectFromThrottleCurve() { StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + Q_ASSERT(stabSettings); QList curve = ui->thrustPIDScalingCurve->getCurve(); @@ -191,12 +193,13 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve() } stabSettings->setEnableThrustPIDScaling(ui->enableThrustPIDScalingCheckBox->isChecked() ? - StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE : StabilizationSettings::ENABLETHRUSTPIDSCALING_FALSE); + StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE : StabilizationSettings::ENABLETHRUSTPIDSCALING_FALSE); } void ConfigStabilizationWidget::resetThrottleCurveToDefault() { StabilizationSettings defaultSettings; + QList curve; for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { curve.append(defaultSettings.getThrustPIDScaleCurve(i)); From c0152b7e192339b7917ad8ffc5860ae06262c49a Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 6 Sep 2014 20:02:22 +0200 Subject: [PATCH 05/21] OP-1474 Thrust PID scaling --- flight/libraries/math/pid.c | 54 ++++++++++++ flight/libraries/math/pid.h | 11 +++ flight/modules/Stabilization/innerloop.c | 87 ++++++++++++++++++- .../boards/coptercontrol/firmware/Makefile | 1 + .../discoveryf4bare/firmware/UAVObjects.inc | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 6 +- .../stabilizationsettings.xml | 4 + shared/uavobjectdefinition/tpsdebug.xml | 15 ++++ 11 files changed, 179 insertions(+), 3 deletions(-) create mode 100644 shared/uavobjectdefinition/tpsdebug.xml diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index c9bcf1494..1ae1e93aa 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -140,3 +140,57 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->d = d; pid->iLim = iLim; } + +float pid_scale_factor_from_line(float x, struct point *p0, struct point *p1) +{ + // Setup line y = m * x + b. + const float dY1 = p1->y - p0->y; + const float dX1 = p1->x - p0->x; + const float m = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==> + const float b = p0->y - m * p0->x; + + // Scale according to given x. + float y = m * x + b; + + return 1.0f + y; +} + +float pid_scale_factor(pid_scaler *scaler) +{ + const int length = sizeof(scaler->points) / sizeof(typeof(scaler->points[0])); + + // Find the two points where is within scaler->x. Use the outer points if + // scaler->x is smaller than the first x value or larger than the last x value. + int end_point = length - 1; + + for (int i = 1; i < length; i++) { + if (scaler->x < scaler->points[i].x) { + end_point = i; + break; + } + } + + return pid_scale_factor_from_line(scaler->x, &scaler->points[end_point - 1], &scaler->points[end_point]); +} + +float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT, + pid_scaler *scaler) +{ + // Save PD values. + float p = pid->p; + float d = pid->d; + + // Scale PD values. + float scale = pid_scale_factor(scaler); + + pid->p *= scale; + pid->d *= scale; + + float value = pid_apply_setpoint(pid, factor, setpoint, measured, dT); + + // Restore PD values. + pid->p = p; + pid->d = d; + + return value; +} diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 6b3c60a43..64dbe2aa8 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -42,11 +42,22 @@ struct pid { float lastDer; }; +typedef struct pid_scaler { + float x; + struct point { + float x; + float y; + } points[5]; +} pid_scaler; + // ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT); +float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT, + pid_scaler *scaler); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); +float pid_scale_factor(pid_scaler *scaler); #endif /* PID_H */ diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 68d6499da..3b0e4cc4b 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -42,6 +42,10 @@ #include #include #include +#include +#include +#include +#include // FIXME: Temporary debugging #include #include @@ -80,6 +84,9 @@ void stabilizationInnerloopInit() StabilizationStatusInitialize(); FlightStatusInitialize(); ManualControlCommandInitialize(); + StabilizationDesiredInitialize(); + ActuatorDesiredInitialize(); + TPSDebugInitialize(); #ifdef REVOLUTION AirspeedStateInitialize(); AirspeedStateConnectCallback(AirSpeedUpdatedCb); @@ -93,6 +100,79 @@ void stabilizationInnerloopInit() PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); } +static float get_pid_scale_source_value() +{ + float value; + + switch (stabSettings.settings.ThrustPIDScaleSource) { + case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE: + ManualControlCommandThrottleGet(&value); + break; + case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST: + StabilizationDesiredThrustGet(&value); + break; + case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIRETHRUST: + ActuatorDesiredThrustGet(&value); + break; + default: + ManualControlCommandThrottleGet(&value); + break; + } + + if (value < 0) { + value = 0.0f; + } + + return value; +} + +static inline pid_scaler create_pid_scaler() +{ + float throttle; + + ManualControlCommandThrottleGet(&throttle); + + struct pid_scaler scaler = { + .x = get_pid_scale_source_value(), + .points = { + { 0.0f, stabSettings.settings.ThrustPIDScaleCurve[0] }, + { 0.25f, stabSettings.settings.ThrustPIDScaleCurve[1] }, + { 0.50f, stabSettings.settings.ThrustPIDScaleCurve[2] }, + { 0.75f, stabSettings.settings.ThrustPIDScaleCurve[3] }, + { 1.0f, stabSettings.settings.ThrustPIDScaleCurve[4] } + } + }; + + // FIXME: Temporary debugging + static int pidDebugCount = 0; + + pidDebugCount++; + if (pidDebugCount == 100) { + struct pid *pid = &stabSettings.innerPids[0]; + + TPSDebugData tpsDebug; + TPSDebugGet(&tpsDebug); + + tpsDebug.thrust = scaler.x; + tpsDebug.p = pid->p; + tpsDebug.d = pid->d; + tpsDebug.factor = pid_scale_factor(&scaler); + tpsDebug.p_scaled = tpsDebug.p * tpsDebug.factor; + tpsDebug.d_scaled = tpsDebug.d * tpsDebug.factor; + + TPSDebugSet(&tpsDebug); + pidDebugCount = 0; + } + + return scaler; +} + +static int is_pid_scaled_for_axis(int axis) +{ + return stabSettings.settings.EnableThrustPIDScaling + && (axis == 0 // Roll + || axis == 1); // Pitch +} /** * WARNING! This callback executes with critical flight control priority every @@ -200,7 +280,12 @@ static void stabilizationInnerloopTask() -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); - actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); + if (is_pid_scaled_for_axis(t)) { + pid_scaler scaler = create_pid_scaler(); + actuatorDesiredAxis[t] = pid_apply_setpoint_scaled(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT, &scaler); + } else { + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); + } break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: default: diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 7a5a7e90f..96fb220bc 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -128,6 +128,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/airspeedstate.c SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c SRC += $(OPUAVSYNTHDIR)/perfcounter.c + SRC += $(OPUAVSYNTHDIR)/tpsdebug.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 49867c39e..c98c9fd71 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -115,6 +115,7 @@ UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 49867c39e..c98c9fd71 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -115,6 +115,7 @@ UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 3905af42f..40db9d9d2 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -114,6 +114,7 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 907ff6833..2e9eda147 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -112,6 +112,7 @@ UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 0f08a3536..1435e7deb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -127,7 +127,8 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ - $$UAVOBJECT_SYNTHETICS/perfcounter.h + $$UAVOBJECT_SYNTHETICS/perfcounter.h \ + $$UAVOBJECT_SYNTHETICS/tpsdebug.h SOURCES += \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \ @@ -231,5 +232,6 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ - $$UAVOBJECT_SYNTHETICS/perfcounter.cpp + $$UAVOBJECT_SYNTHETICS/perfcounter.cpp \ + $$UAVOBJECT_SYNTHETICS/tpsdebug.cpp diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index a79798b4c..4156cef76 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -47,6 +47,10 @@ + + + + diff --git a/shared/uavobjectdefinition/tpsdebug.xml b/shared/uavobjectdefinition/tpsdebug.xml new file mode 100644 index 000000000..dcc34255f --- /dev/null +++ b/shared/uavobjectdefinition/tpsdebug.xml @@ -0,0 +1,15 @@ + + + Debugging of the @ref Thrust PID Scale module + + + + + + + + + + + + From 0e2af9c654a9bdfb53766fce73e4b29c6a36bb4f Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 10 Sep 2014 00:34:06 +0200 Subject: [PATCH 06/21] OP-1474 Extract line and curve functions out to mathmisc.h, nan check and unit tests --- Makefile | 2 +- flight/libraries/math/mathmisc.h | 40 +++++++++++ flight/libraries/math/pid.c | 29 +------- flight/libraries/math/pid.h | 9 ++- flight/tests/math/Makefile | 37 +++++++++++ flight/tests/math/unittest.cpp | 110 +++++++++++++++++++++++++++++++ 6 files changed, 194 insertions(+), 33 deletions(-) create mode 100644 flight/tests/math/Makefile create mode 100644 flight/tests/math/unittest.cpp diff --git a/Makefile b/Makefile index 89a7d1cd6..ef773096b 100644 --- a/Makefile +++ b/Makefile @@ -638,7 +638,7 @@ uavo-collections_clean: # ############################## -ALL_UNITTESTS := logfs +ALL_UNITTESTS := logfs math # Build the directory for the unit tests UT_OUT_DIR := $(BUILD_DIR)/unit_tests diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 42213e504..823d358b9 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -31,6 +31,8 @@ #ifndef MATHMISC_H #define MATHMISC_H +#include + // returns min(boundary1,boundary2) if valmax(boundary1,boundary2) // returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2) @@ -79,4 +81,42 @@ static inline void vector_normalizef(float *vector, const uint8_t dim) } } +typedef struct pointf { + float x; + float y; +} pointf; + +// Returns the y value, given x, on the line passing through the points p0 and p1. +static inline float y_on_line(float x, pointf *p0, pointf *p1) +{ + // Setup line y = m * x + b. + const float dY1 = p1->y - p0->y; + const float dX1 = p1->x - p0->x; + const float m = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==> + const float b = p0->y - m * p0->x; + + // Get the y value on the line. + return m * x + b; +} + +// Returns the y value, given x, on the curve defined by the points array. +// The fist and last line of the curve extends beyond the first resp. last points. +static inline float y_on_curve(float x, pointf points[], int num_points) +{ + // Find the two points x is within. + // If x is smaller than the first point's x value, use the first line of the curve. + // If x is larger than the last point's x value, user the last line of the curve. + int end_point = num_points - 1; + + for (int i = 1; i < num_points; i++) { + if (x < points[i].x) { + end_point = i; + break; + } + } + + // Find the y value on the selected line. + return y_on_line(x, &points[end_point - 1], &points[end_point]); +} + #endif /* MATHMISC_H */ diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 1ae1e93aa..0523b16dd 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -141,36 +141,11 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->iLim = iLim; } -float pid_scale_factor_from_line(float x, struct point *p0, struct point *p1) -{ - // Setup line y = m * x + b. - const float dY1 = p1->y - p0->y; - const float dX1 = p1->x - p0->x; - const float m = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==> - const float b = p0->y - m * p0->x; - - // Scale according to given x. - float y = m * x + b; - - return 1.0f + y; -} - float pid_scale_factor(pid_scaler *scaler) { - const int length = sizeof(scaler->points) / sizeof(typeof(scaler->points[0])); + float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0])); - // Find the two points where is within scaler->x. Use the outer points if - // scaler->x is smaller than the first x value or larger than the last x value. - int end_point = length - 1; - - for (int i = 1; i < length; i++) { - if (scaler->x < scaler->points[i].x) { - end_point = i; - break; - } - } - - return pid_scale_factor_from_line(scaler->x, &scaler->points[end_point - 1], &scaler->points[end_point]); + return 1.0f + (IS_REAL(y) ? y : 0.0f); } float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT, diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 64dbe2aa8..cf99e090a 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -31,6 +31,8 @@ #ifndef PID_H #define PID_H +#include "mathmisc.h" + // ! struct pid { float p; @@ -43,11 +45,8 @@ struct pid { }; typedef struct pid_scaler { - float x; - struct point { - float x; - float y; - } points[5]; + float x; + pointf points[5]; } pid_scaler; // ! Methods to use the pid structures diff --git a/flight/tests/math/Makefile b/flight/tests/math/Makefile new file mode 100644 index 000000000..cb97e89f9 --- /dev/null +++ b/flight/tests/math/Makefile @@ -0,0 +1,37 @@ +############################################################################### +# @file Makefile +# @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 +# Copyright (c) 2013, The OpenPilot Team, http://www.openpilot.org +# @addtogroup +# @{ +# @addtogroup +# @{ +# @brief Makefile for unit test +############################################################################### +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + +ifndef OPENPILOT_IS_COOL + $(error Top level Makefile must be used to build this target) +endif + +include $(ROOT_DIR)/make/firmware-defs.mk + +EXTRAINCDIRS += $(TOPDIR) +EXTRAINCDIRS += $(ROOT_DIR)/flight/libraries/math +EXTRAINCDIRS += $(PIOS)/inc + +include $(ROOT_DIR)/make/unittest.mk diff --git a/flight/tests/math/unittest.cpp b/flight/tests/math/unittest.cpp new file mode 100644 index 000000000..e6523d0c3 --- /dev/null +++ b/flight/tests/math/unittest.cpp @@ -0,0 +1,110 @@ +#include "gtest/gtest.h" + +#include /* printf */ +#include /* abort */ +#include /* memset */ + +extern "C" { +#include "mathmisc.h" +} + +#define epsilon 0.00001f +// From pios_math.h +#define IS_REAL(f) (!isnan(f) && !isinf(f)) +#define length(points_array) (sizeof(points_array) / sizeof(points_array[0])) + +// To use a test fixture, derive a class from testing::Test. +class MathTestRaw : public testing::Test {}; + +TEST_F(MathTestRaw, y_on_line0) { + pointf points[] = { + { 0.0f, -0.30f }, + { 0.5f, 0.30 } + }; + + EXPECT_NEAR(-0.60f, y_on_line(-0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(-0.30f, y_on_line(0.00f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.00f, y_on_line(0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.60f, y_on_line(0.75f, &points[0], &points[1]), epsilon); +} + +TEST_F(MathTestRaw, y_on_line1) { + pointf points[] = { + { 0.25f, -0.30f }, + { 0.50f, 0.30 } + }; + + EXPECT_NEAR(-1.50f, y_on_line(-0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(-0.90f, y_on_line(0.00f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(-0.30f, y_on_line(0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.90f, y_on_line(0.75f, &points[0], &points[1]), epsilon); +} + +TEST_F(MathTestRaw, y_on_line2) { + pointf points[] = { + { -0.25f, -0.30f }, + { 0.50f, 0.30 } + }; + + EXPECT_NEAR(-0.30f, y_on_line(-0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(-0.10f, y_on_line(0.00f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.10f, y_on_line(0.25f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.30f, y_on_line(0.50f, &points[0], &points[1]), epsilon); + EXPECT_NEAR(0.50f, y_on_line(0.75f, &points[0], &points[1]), epsilon); +} + +TEST_F(MathTestRaw, y_on_line3) { + pointf points[] = { + { 0.25f, -0.30f }, + { 0.25f, 0.30 } + }; + + + EXPECT_FALSE(IS_REAL(y_on_line(-0.25f, &points[0], &points[1]))); +} + +TEST_F(MathTestRaw, y_on_curve0) { + pointf points[] = { + { 0.00f, -0.40f }, + { 0.25f, -0.20f }, + { 0.50f, 0.00f }, + { 0.75f, 0.20 }, + { 1.00f, 0.40 } + }; + + EXPECT_NEAR(-0.50f, y_on_curve(-0.125f, points, length(points)), epsilon); + EXPECT_NEAR(-0.40f, y_on_curve(0.000f, points, length(points)), epsilon); + EXPECT_NEAR(-0.30f, y_on_curve(0.125f, points, length(points)), epsilon); + EXPECT_NEAR(-0.20f, y_on_curve(0.250f, points, length(points)), epsilon); + EXPECT_NEAR(-0.10f, y_on_curve(0.375f, points, length(points)), epsilon); + EXPECT_NEAR(0.00f, y_on_curve(0.500f, points, length(points)), epsilon); + EXPECT_NEAR(0.10f, y_on_curve(0.625f, points, length(points)), epsilon); + EXPECT_NEAR(0.20f, y_on_curve(0.750f, points, length(points)), epsilon); + EXPECT_NEAR(0.30f, y_on_curve(0.875f, points, length(points)), epsilon); + EXPECT_NEAR(0.40f, y_on_curve(1.000f, points, length(points)), epsilon); + EXPECT_NEAR(0.50f, y_on_curve(1.125f, points, length(points)), epsilon); +} + + +TEST_F(MathTestRaw, y_on_curve1) { + pointf points[] = { + { -0.25f, 0.10f }, + { 0.00f, 0.20f }, + { 0.50f, 0.30f }, + { 1.00f, -0.30 }, + { 2.00f, -0.50 } + }; + + EXPECT_NEAR(0.00f, y_on_curve(-0.500f, points, length(points)), epsilon); + EXPECT_NEAR(0.10f, y_on_curve(-0.250f, points, length(points)), epsilon); + EXPECT_NEAR(0.15f, y_on_curve(-0.125f, points, length(points)), epsilon); + EXPECT_NEAR(0.20f, y_on_curve(0.000f, points, length(points)), epsilon); + EXPECT_NEAR(0.22f, y_on_curve(0.100f, points, length(points)), epsilon); + EXPECT_NEAR(0.30f, y_on_curve(0.500f, points, length(points)), epsilon); + EXPECT_NEAR(0.00f, y_on_curve(0.750f, points, length(points)), epsilon); + EXPECT_NEAR(-0.30f, y_on_curve(1.000f, points, length(points)), epsilon); + EXPECT_NEAR(-0.35f, y_on_curve(1.250f, points, length(points)), epsilon); + EXPECT_NEAR(-0.50f, y_on_curve(2.000f, points, length(points)), epsilon); +} From 7afb2113307fb4eb0200f8a0ad3a8f443bf3d323 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 10 Sep 2014 00:42:40 +0200 Subject: [PATCH 07/21] OP-1474 Revert TPS debugging code --- flight/modules/Stabilization/innerloop.c | 24 ------------------- .../boards/coptercontrol/firmware/Makefile | 1 - .../discoveryf4bare/firmware/UAVObjects.inc | 1 - .../boards/revolution/firmware/UAVObjects.inc | 1 - .../boards/revoproto/firmware/UAVObjects.inc | 1 - .../boards/simposix/firmware/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 6 ++--- shared/uavobjectdefinition/tpsdebug.xml | 15 ------------ 8 files changed, 2 insertions(+), 48 deletions(-) delete mode 100644 shared/uavobjectdefinition/tpsdebug.xml diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 3b0e4cc4b..3ce0fb4e5 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -44,8 +44,6 @@ #include #include #include -#include -#include // FIXME: Temporary debugging #include #include @@ -86,7 +84,6 @@ void stabilizationInnerloopInit() ManualControlCommandInitialize(); StabilizationDesiredInitialize(); ActuatorDesiredInitialize(); - TPSDebugInitialize(); #ifdef REVOLUTION AirspeedStateInitialize(); AirspeedStateConnectCallback(AirSpeedUpdatedCb); @@ -143,27 +140,6 @@ static inline pid_scaler create_pid_scaler() } }; - // FIXME: Temporary debugging - static int pidDebugCount = 0; - - pidDebugCount++; - if (pidDebugCount == 100) { - struct pid *pid = &stabSettings.innerPids[0]; - - TPSDebugData tpsDebug; - TPSDebugGet(&tpsDebug); - - tpsDebug.thrust = scaler.x; - tpsDebug.p = pid->p; - tpsDebug.d = pid->d; - tpsDebug.factor = pid_scale_factor(&scaler); - tpsDebug.p_scaled = tpsDebug.p * tpsDebug.factor; - tpsDebug.d_scaled = tpsDebug.d * tpsDebug.factor; - - TPSDebugSet(&tpsDebug); - pidDebugCount = 0; - } - return scaler; } diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 96fb220bc..7a5a7e90f 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -128,7 +128,6 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/airspeedstate.c SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c SRC += $(OPUAVSYNTHDIR)/perfcounter.c - SRC += $(OPUAVSYNTHDIR)/tpsdebug.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index c98c9fd71..49867c39e 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -115,7 +115,6 @@ UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index c98c9fd71..49867c39e 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -115,7 +115,6 @@ UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 40db9d9d2..3905af42f 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -114,7 +114,6 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += takeofflocation -UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 2e9eda147..907ff6833 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -112,7 +112,6 @@ UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation -UAVOBJSRCFILENAMES += tpsdebug UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 1435e7deb..0f08a3536 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -127,8 +127,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \ $$UAVOBJECT_SYNTHETICS/takeofflocation.h \ - $$UAVOBJECT_SYNTHETICS/perfcounter.h \ - $$UAVOBJECT_SYNTHETICS/tpsdebug.h + $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \ @@ -232,6 +231,5 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \ $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \ - $$UAVOBJECT_SYNTHETICS/perfcounter.cpp \ - $$UAVOBJECT_SYNTHETICS/tpsdebug.cpp + $$UAVOBJECT_SYNTHETICS/perfcounter.cpp diff --git a/shared/uavobjectdefinition/tpsdebug.xml b/shared/uavobjectdefinition/tpsdebug.xml deleted file mode 100644 index dcc34255f..000000000 --- a/shared/uavobjectdefinition/tpsdebug.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - Debugging of the @ref Thrust PID Scale module - - - - - - - - - - - - From 344172e87927e222ca4c692a759ebbf26e0b497c Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 10 Sep 2014 01:03:49 +0200 Subject: [PATCH 08/21] OP-1474 Add missing stdint.h include to mathmisc.h --- flight/libraries/math/mathmisc.h | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 823d358b9..aa65746ed 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -32,6 +32,7 @@ #define MATHMISC_H #include +#include // returns min(boundary1,boundary2) if valmax(boundary1,boundary2) From 662b29ac05c6ba1c27ae56afbe3c5d15ce5232e1 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 10 Sep 2014 22:02:21 +0200 Subject: [PATCH 09/21] OP-1474 Include I in the scaling and let the TPS factor piggyback on the speedScaleFactor --- flight/libraries/math/pid.c | 22 ---------------------- flight/libraries/math/pid.h | 2 -- flight/modules/Stabilization/innerloop.c | 15 +++++---------- 3 files changed, 5 insertions(+), 34 deletions(-) diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 0523b16dd..9b28b35ae 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -147,25 +147,3 @@ float pid_scale_factor(pid_scaler *scaler) return 1.0f + (IS_REAL(y) ? y : 0.0f); } - -float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT, - pid_scaler *scaler) -{ - // Save PD values. - float p = pid->p; - float d = pid->d; - - // Scale PD values. - float scale = pid_scale_factor(scaler); - - pid->p *= scale; - pid->d *= scale; - - float value = pid_apply_setpoint(pid, factor, setpoint, measured, dT); - - // Restore PD values. - pid->p = p; - pid->d = d; - - return value; -} diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index cf99e090a..72f1a8b8c 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -52,8 +52,6 @@ typedef struct pid_scaler { // ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT); -float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT, - pid_scaler *scaler); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 3ce0fb4e5..4311ea8b4 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -123,12 +123,8 @@ static float get_pid_scale_source_value() return value; } -static inline pid_scaler create_pid_scaler() +static float get_pid_scale_factor() { - float throttle; - - ManualControlCommandThrottleGet(&throttle); - struct pid_scaler scaler = { .x = get_pid_scale_source_value(), .points = { @@ -140,7 +136,7 @@ static inline pid_scaler create_pid_scaler() } }; - return scaler; + return pid_scale_factor(&scaler); } static int is_pid_scaled_for_axis(int axis) @@ -256,12 +252,11 @@ static void stabilizationInnerloopTask() -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); + float scaleFactor = speedScaleFactor; if (is_pid_scaled_for_axis(t)) { - pid_scaler scaler = create_pid_scaler(); - actuatorDesiredAxis[t] = pid_apply_setpoint_scaled(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT, &scaler); - } else { - actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); + scaleFactor *= get_pid_scale_factor(); } + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT); break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: default: From bdbe875a909fa5c6af3cdde34f76c263b2965243 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sun, 14 Sep 2014 16:08:21 +0200 Subject: [PATCH 10/21] OP-1474 Use ActuatorDesiredThrust as default source for TPS --- flight/modules/Stabilization/innerloop.c | 4 ++-- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 4311ea8b4..db367e0c1 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -108,11 +108,11 @@ static float get_pid_scale_source_value() case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST: StabilizationDesiredThrustGet(&value); break; - case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIRETHRUST: + case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST: ActuatorDesiredThrustGet(&value); break; default: - ManualControlCommandThrottleGet(&value); + ActuatorDesiredThrustGet(&value); break; } diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 4156cef76..be805b737 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -49,7 +49,7 @@ - + From d3e7ef300f7c6b4761f5b2e46d684c291b1f6963 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Thu, 18 Sep 2014 22:36:53 +0200 Subject: [PATCH 11/21] OP-1474 Use PID banks for the TPS settings --- flight/modules/Stabilization/innerloop.c | 20 +++++++++---------- .../uavobjectdefinition/stabilizationbank.xml | 5 +++++ .../stabilizationsettingsbank1.xml | 5 +++++ .../stabilizationsettingsbank2.xml | 5 +++++ .../stabilizationsettingsbank3.xml | 7 ++++++- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index db367e0c1..1a71e9679 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -101,14 +101,14 @@ static float get_pid_scale_source_value() { float value; - switch (stabSettings.settings.ThrustPIDScaleSource) { - case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE: + switch (stabSettings.stabBank.ThrustPIDScaleSource) { + case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE: ManualControlCommandThrottleGet(&value); break; - case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST: + case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST: StabilizationDesiredThrustGet(&value); break; - case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST: + case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST: ActuatorDesiredThrustGet(&value); break; default: @@ -128,11 +128,11 @@ static float get_pid_scale_factor() struct pid_scaler scaler = { .x = get_pid_scale_source_value(), .points = { - { 0.0f, stabSettings.settings.ThrustPIDScaleCurve[0] }, - { 0.25f, stabSettings.settings.ThrustPIDScaleCurve[1] }, - { 0.50f, stabSettings.settings.ThrustPIDScaleCurve[2] }, - { 0.75f, stabSettings.settings.ThrustPIDScaleCurve[3] }, - { 1.0f, stabSettings.settings.ThrustPIDScaleCurve[4] } + { 0.0f, stabSettings.stabBank.ThrustPIDScaleCurve[0] }, + { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] }, + { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] }, + { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] }, + { 1.0f, stabSettings.stabBank.ThrustPIDScaleCurve[4] } } }; @@ -141,7 +141,7 @@ static float get_pid_scale_factor() static int is_pid_scaled_for_axis(int axis) { - return stabSettings.settings.EnableThrustPIDScaling + return stabSettings.stabBank.EnableThrustPIDScaling && (axis == 0 // Roll || axis == 1); // Pitch } diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index 027d19372..100cf012f 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -14,6 +14,11 @@ + + + + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index 7c8a71e64..bffd12fe7 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -14,6 +14,11 @@ + + + + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 75b777fa5..3f41a308c 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -14,6 +14,11 @@ + + + + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 7b0d52e28..55bd6ba69 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -8,12 +8,17 @@ - + + + + + + From 31c5f1c519f9159252d3905beb147a21c778ac6d Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 19 Sep 2014 00:26:09 +0200 Subject: [PATCH 12/21] OP-1474 Make it possible to select the combination of P, I, and D to scale --- flight/libraries/math/mathmisc.h | 4 +- flight/libraries/math/pid.c | 15 +-- flight/libraries/math/pid.h | 8 +- flight/modules/Stabilization/altitudeloop.c | 8 +- flight/modules/Stabilization/innerloop.c | 102 ++++++++++++++---- .../uavobjectdefinition/stabilizationbank.xml | 1 + .../stabilizationsettingsbank1.xml | 1 + .../stabilizationsettingsbank2.xml | 1 + .../stabilizationsettingsbank3.xml | 1 + 9 files changed, 100 insertions(+), 41 deletions(-) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index aa65746ed..0fc95f963 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -88,7 +88,7 @@ typedef struct pointf { } pointf; // Returns the y value, given x, on the line passing through the points p0 and p1. -static inline float y_on_line(float x, pointf *p0, pointf *p1) +static inline float y_on_line(float x, const pointf *p0, const pointf *p1) { // Setup line y = m * x + b. const float dY1 = p1->y - p0->y; @@ -102,7 +102,7 @@ static inline float y_on_line(float x, pointf *p0, pointf *p1) // Returns the y value, given x, on the curve defined by the points array. // The fist and last line of the curve extends beyond the first resp. last points. -static inline float y_on_curve(float x, pointf points[], int num_points) +static inline float y_on_curve(float x, const pointf points[], int num_points) { // Find the two points x is within. // If x is smaller than the first point's x value, use the first line of the curve. diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 9b28b35ae..6e78078ef 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -76,12 +76,12 @@ float pid_apply(struct pid *pid, const float err, float dT) * This version of apply uses setpoint weighting for the derivative component so the gain * on the gyro derivative can be different than the gain on the setpoint derivative */ -float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT) +float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT) { float err = setpoint - measured; // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f); + pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f); pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f); // Calculate DT1 term, @@ -89,11 +89,11 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); pid->lastErr = (deriv_gamma * setpoint - measured); if (pid->d > 0.0f && dT > 0.0f) { - dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer); + dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) } // 7.9577e-3 means 20 Hz f_cutoff - return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm; + return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm; } /** @@ -140,10 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->d = d; pid->iLim = iLim; } - -float pid_scale_factor(pid_scaler *scaler) -{ - float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0])); - - return 1.0f + (IS_REAL(y) ? y : 0.0f); -} diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 72f1a8b8c..0031fb212 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -45,16 +45,16 @@ struct pid { }; typedef struct pid_scaler { - float x; - pointf points[5]; + float p; + float i; + float d; } pid_scaler; // ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); -float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT); +float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); -float pid_scale_factor(pid_scaler *scaler); #endif /* PID_H */ diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.c index 0ca4e7f01..329990d0e 100644 --- a/flight/modules/Stabilization/altitudeloop.c +++ b/flight/modules/Stabilization/altitudeloop.c @@ -164,7 +164,9 @@ static void altitudeHoldTask(void) switch (thrustMode) { case ALTITUDEHOLD: // altitude control loop - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT); + // No scaling. + pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; + altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT); break; case ALTITUDEVARIO: altitudeHoldStatus.VelocityDesired = thrustSetpoint; @@ -182,7 +184,9 @@ static void altitudeHoldTask(void) break; default: // velocity control loop - thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); + // No scaling. + pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; + thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); break; } diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 1a71e9679..90a1b8b52 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -123,29 +123,90 @@ static float get_pid_scale_source_value() return value; } -static float get_pid_scale_factor() -{ - struct pid_scaler scaler = { - .x = get_pid_scale_source_value(), - .points = { - { 0.0f, stabSettings.stabBank.ThrustPIDScaleCurve[0] }, - { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] }, - { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] }, - { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] }, - { 1.0f, stabSettings.stabBank.ThrustPIDScaleCurve[4] } - } - }; - - return pid_scale_factor(&scaler); -} - -static int is_pid_scaled_for_axis(int axis) +static int is_pid_thrust_scaled_for_axis(int axis) { return stabSettings.stabBank.EnableThrustPIDScaling && (axis == 0 // Roll || axis == 1); // Pitch } +static bool is_p_scaling_enabled() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P; +} + +static bool is_i_scaling_enabled() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I; +} + +static bool is_d_scaling_enabled() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D; +} + +typedef struct pid_curve_scaler { + float x; + pointf points[5]; +} pid_curve_scaler; + +static float pid_curve_value(const pid_curve_scaler *scaler) +{ + float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0])); + + return 1.0f + (IS_REAL(y) ? y : 0.0f); +} + +static pid_scaler create_pid_scaler(int axis) +{ + pid_scaler scaler; + + // Always scaled with the this. + scaler.p = scaler.i = scaler.d = speedScaleFactor; + + if (is_pid_thrust_scaled_for_axis(axis)) { + const pid_curve_scaler curve_scaler = { + .x = get_pid_scale_source_value(), + .points = { + { 0.00f, stabSettings.stabBank.ThrustPIDScaleCurve[0] }, + { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] }, + { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] }, + { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] }, + { 1.00f, stabSettings.stabBank.ThrustPIDScaleCurve[4] } + } + }; + + float curve_value = pid_curve_value(&curve_scaler); + + if (is_p_scaling_enabled()) { + scaler.p *= curve_value; + } + if (is_i_scaling_enabled()) { + scaler.i *= curve_value; + } + if (is_d_scaling_enabled()) { + scaler.d *= curve_value; + } + } + + return scaler; +} + /** * WARNING! This callback executes with critical flight control priority every * time a gyroscope update happens do NOT put any time consuming calculations @@ -252,11 +313,8 @@ static void stabilizationInnerloopTask() -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); - float scaleFactor = speedScaleFactor; - if (is_pid_scaled_for_axis(t)) { - scaleFactor *= get_pid_scale_factor(); - } - actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT); + pid_scaler scaler = create_pid_scaler(t); + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT); break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: default: diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index 100cf012f..4689fa025 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -18,6 +18,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index bffd12fe7..ef5cd53d3 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -18,6 +18,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 3f41a308c..12b8e06d4 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -18,6 +18,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 55bd6ba69..eb48029d6 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -18,6 +18,7 @@ + From f0c47976a5be81854becc9d75918400b026ca6af Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 19 Sep 2014 01:29:04 +0200 Subject: [PATCH 13/21] OP-1474 Fix label compile error --- flight/modules/Stabilization/altitudeloop.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.c index 329990d0e..f54ed5740 100644 --- a/flight/modules/Stabilization/altitudeloop.c +++ b/flight/modules/Stabilization/altitudeloop.c @@ -163,11 +163,13 @@ static void altitudeHoldTask(void) dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); switch (thrustMode) { case ALTITUDEHOLD: + { // altitude control loop // No scaling. - pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; + const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT); - break; + } + break; case ALTITUDEVARIO: altitudeHoldStatus.VelocityDesired = thrustSetpoint; break; @@ -183,12 +185,13 @@ static void altitudeHoldTask(void) thrustDemand = thrustSetpoint; break; default: + { // velocity control loop // No scaling. - pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; + const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); - - break; + } + break; } } From 9a21eed71e255c9856282c0974555d8992c14f41 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 19 Sep 2014 10:07:11 +0200 Subject: [PATCH 14/21] OP-1474 Make it possible to select axes to scale --- flight/modules/Stabilization/innerloop.c | 35 +++++++++++++++++-- .../uavobjectdefinition/stabilizationbank.xml | 1 + .../stabilizationsettingsbank1.xml | 1 + .../stabilizationsettingsbank2.xml | 1 + .../stabilizationsettingsbank3.xml | 1 + 5 files changed, 37 insertions(+), 2 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 90a1b8b52..9e0a80e83 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -123,11 +123,42 @@ static float get_pid_scale_source_value() return value; } +static bool is_roll_pid_thrust_scaled() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL; +} + +static bool is_pitch_pid_thrust_scaled() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH; +} + +static bool is_yaw_pid_thrust_scaled() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW; +} + static int is_pid_thrust_scaled_for_axis(int axis) { return stabSettings.stabBank.EnableThrustPIDScaling - && (axis == 0 // Roll - || axis == 1); // Pitch + && ((axis == 0 && is_roll_pid_thrust_scaled()) + || (axis == 1 && is_pitch_pid_thrust_scaled()) + || (axis == 2 && is_yaw_pid_thrust_scaled())); } static bool is_p_scaling_enabled() diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index 4689fa025..b1fbe5c13 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -19,6 +19,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index ef5cd53d3..8226eadd5 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -19,6 +19,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 12b8e06d4..f546c9ff2 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -19,6 +19,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index eb48029d6..c534f1dce 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -19,6 +19,7 @@ + From b3e7a23f4fa9701b8b693f7f2d68b21d9ad76553 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 19 Sep 2014 15:08:07 +0200 Subject: [PATCH 15/21] OP-1474 Demultiplex TPS settings when pid banks are updated --- .../modules/Stabilization/inc/stabilization.h | 2 + flight/modules/Stabilization/innerloop.c | 78 ++----------------- flight/modules/Stabilization/stabilization.c | 78 +++++++++++++++++++ 3 files changed, 86 insertions(+), 72 deletions(-) diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 44ab8917c..8fc084da2 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -59,6 +59,8 @@ typedef struct { } monitor; float rattitude_mode_transition_stick_position; struct pid innerPids[3], outerPids[3]; + // TPS [Roll,Pitch,Yaw][P,I,D] + bool thrust_pid_scaling_enabled[3][3]; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 9e0a80e83..1f152c382 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -123,74 +123,6 @@ static float get_pid_scale_source_value() return value; } -static bool is_roll_pid_thrust_scaled() -{ - uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; - - return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL; -} - -static bool is_pitch_pid_thrust_scaled() -{ - uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; - - return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH; -} - -static bool is_yaw_pid_thrust_scaled() -{ - uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; - - return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || - axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW; -} - -static int is_pid_thrust_scaled_for_axis(int axis) -{ - return stabSettings.stabBank.EnableThrustPIDScaling - && ((axis == 0 && is_roll_pid_thrust_scaled()) - || (axis == 1 && is_pitch_pid_thrust_scaled()) - || (axis == 2 && is_yaw_pid_thrust_scaled())); -} - -static bool is_p_scaling_enabled() -{ - uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; - - return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P; -} - -static bool is_i_scaling_enabled() -{ - uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; - - return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I; -} - -static bool is_d_scaling_enabled() -{ - uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; - - return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || - target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D; -} - typedef struct pid_curve_scaler { float x; pointf points[5]; @@ -210,7 +142,9 @@ static pid_scaler create_pid_scaler(int axis) // Always scaled with the this. scaler.p = scaler.i = scaler.d = speedScaleFactor; - if (is_pid_thrust_scaled_for_axis(axis)) { + if (stabSettings.thrust_pid_scaling_enabled[axis][0] + || stabSettings.thrust_pid_scaling_enabled[axis][1] + || stabSettings.thrust_pid_scaling_enabled[axis][2]) { const pid_curve_scaler curve_scaler = { .x = get_pid_scale_source_value(), .points = { @@ -224,13 +158,13 @@ static pid_scaler create_pid_scaler(int axis) float curve_value = pid_curve_value(&curve_scaler); - if (is_p_scaling_enabled()) { + if (stabSettings.thrust_pid_scaling_enabled[axis][0]) { scaler.p *= curve_value; } - if (is_i_scaling_enabled()) { + if (stabSettings.thrust_pid_scaling_enabled[axis][1]) { scaler.i *= curve_value; } - if (is_d_scaling_enabled()) { + if (stabSettings.thrust_pid_scaling_enabled[axis][2]) { scaler.d *= curve_value; } } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 202a5c89f..074496983 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -229,6 +229,66 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) StabilizationBankSet(&stabSettings.stabBank); } +static bool use_tps_for_roll() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL; +} + +static bool use_tps_for_pitch() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH; +} + +static bool use_tps_for_yaw() +{ + uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes; + + return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW || + axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW; +} + +static bool use_tps_for_p() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P; +} + +static bool use_tps_for_i() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I; +} + +static bool use_tps_for_d() +{ + uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget; + + return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID || + target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D; +} + static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { StabilizationBankGet(&stabSettings.stabBank); @@ -268,6 +328,24 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) stabSettings.stabBank.YawPI.Ki, 0, stabSettings.stabBank.YawPI.ILimit); + + bool tps_for_axis[3] = { + use_tps_for_roll(), + use_tps_for_pitch(), + use_tps_for_yaw() + }; + bool tps_for_pid[3] = { + use_tps_for_p(), + use_tps_for_i(), + use_tps_for_d() + }; + for (int axis = 0; axis < 3; axis++) { + for (int pid = 0; pid < 3; pid++) { + stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling + && tps_for_axis[axis] + && tps_for_pid[pid]; + } + } } From 76b45ea87ae33a5fc8575c446f4d27d58110d0ce Mon Sep 17 00:00:00 2001 From: m_thread Date: Fri, 19 Sep 2014 15:14:29 +0200 Subject: [PATCH 16/21] OP-1474 Moving Thrust PID Curve to PID banks. Adding ThrustPIDScalingSource, ThrustPIDScalingTargets and ThrustPIDScalingAxis. --- .../config/cfg_vehicletypes/vehicleconfig.cpp | 2 +- .../src/plugins/config/stabilization.ui | 2119 ++++++++++++++++- 2 files changed, 2003 insertions(+), 118 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index 1ebfba307..b6486e5cf 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -73,7 +73,7 @@ GUIConfigDataUnion VehicleConfig::getConfigData() } // sanity check - Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); + //Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); return configData; } diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 0bab6ff55..26d274637 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -95,7 +95,7 @@ QTabWidget::Rounded - 2 + 0 false @@ -136,8 +136,8 @@ 0 0 - 815 - 708 + 801 + 726 @@ -250,7 +250,7 @@ margin-top: -1px; 0 - 16 + 20 @@ -933,7 +933,7 @@ border-radius: 5; 0 - 16 + 20 @@ -1486,7 +1486,7 @@ border-radius: 5; 0 - 16 + 20 @@ -2931,7 +2931,7 @@ value as the Kp. 0 - 16 + 20 @@ -3507,7 +3507,7 @@ border-radius: 5; 0 - 16 + 20 @@ -4057,7 +4057,7 @@ border-radius: 5; 0 - 16 + 20 @@ -5659,7 +5659,7 @@ Then lower the value by 5 or so. 0 - 16 + 20 @@ -6209,7 +6209,7 @@ border-radius: 5; 0 - 16 + 20 @@ -6759,7 +6759,7 @@ border-radius: 5; 0 - 16 + 20 @@ -7836,9 +7836,9 @@ border-radius: 5; 0 - 0 - 815 - 708 + -299 + 801 + 998 @@ -8689,7 +8689,7 @@ border-radius: 5; 0 - 16 + 20 @@ -9271,7 +9271,7 @@ border-radius: 5; 0 - 16 + 20 @@ -9915,7 +9915,7 @@ border-radius: 5; 0 - 16 + 20 @@ -11281,7 +11281,7 @@ border-radius: 5; 0 - 16 + 20 @@ -12050,7 +12050,7 @@ border-radius: 5; 0 - 16 + 20 @@ -12600,7 +12600,7 @@ border-radius: 5; 0 - 16 + 20 @@ -14132,7 +14132,7 @@ border-radius: 5; 0 - 16 + 20 @@ -14682,7 +14682,7 @@ border-radius: 5; 0 - 16 + 20 @@ -15232,7 +15232,7 @@ border-radius: 5; 0 - 16 + 20 @@ -16182,6 +16182,1810 @@ border-radius: 5; + + + + Thrust PID Scaling + + + false + + + + + + + 0 + 0 + + + + + 200 + 200 + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:99 + + + + + + + + + + + Qt::Horizontal + + + QSizePolicy::Expanding + + + + 40 + 20 + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + + + Enable Thrust PID Scaling + + + + objname:StabilizationSettingsBankX + fieldname:EnableThrustPIDScaling + buttongroup:99 + + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 39 + 39 + 39 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 75 + true + + + + false + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + Source + + + Qt::AlignCenter + + + + + + + + objname:StabilizationSettingsBankX + fieldname:ThrustPIDScaleSource + buttongroup:99 + + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 39 + 39 + 39 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 75 + true + + + + false + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + Targets + + + Qt::AlignCenter + + + + + + + + objname:StabilizationSettingsBankX + fieldname:ThrustPIDScaleTarget + buttongroup:99 + + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 19 + 19 + 19 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 58 + 58 + 58 + + + + + + + 48 + 48 + 48 + + + + + + + 19 + 19 + 19 + + + + + + + 26 + 26 + 26 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + 255 + 255 + 255 + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + + + 74 + 74 + 74 + + + + + 36 + 36 + 36 + + + + + + + + + 0 + 0 + 0 + + + + + + + 39 + 39 + 39 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 75 + true + + + + false + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + Axis + + + Qt::AlignCenter + + + + + + + + objname:StabilizationSettingsBankX + fieldname:ThrustPIDScaleAxis + buttongroup:99 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + @@ -16299,11 +18103,11 @@ border-radius: 5; 0 0 - 815 - 708 + 817 + 699 - + @@ -18883,22 +20687,6 @@ font:bold; - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 9 - - - - @@ -18915,13 +20703,33 @@ font:bold; Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + 0 6 + + + + + 0 + 0 + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:15 + + + + @@ -18947,30 +20755,17 @@ font:bold; - 0 + 9 - 0 + 9 - 0 + 9 - 0 + 9 - - - - Qt::Vertical - - - - 20 - 5 - - - - @@ -19065,44 +20860,24 @@ border-radius: 5; - - - - Qt::Vertical - - - - 20 - 10 - - - - - - - - - 0 - 0 - + + + + Qt::Vertical - - Default + + + 20 + 40 + - - - objname:StabilizationSettings - button:default - buttongroup:15 - - - + @@ -19157,6 +20932,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>Throttle/Collective stick below this amount disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when InvertedPower setting is Zero and the copter is inverted. CP helis probably want this set to -100%. For safety with fixed pitch copters (including multicopters), never set this so low that the trimmed throttle stick cannot get below it or your motor(s) will still run with the throttle stick all the way down. Fixed pitch throttle sticks go from -100 to 0 in the first tiny bit of throttle stick (and then up to 100 using the rest of the throttle range), so for example, a lot of &quot;high throttle trim&quot; will keep the stick from ever commanding less than 5% so it won't be possible to stop the motors with the throttle stick. Banking the copter in your hand in this state will make the motors speed up.</p></body></html> @@ -19191,6 +20978,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>Multi-copters should probably use 80% to 90% to leave some headroom for stabilization. CP helis can set this to 100%.</p></body></html> @@ -19231,7 +21030,13 @@ border-radius: 5; 97 - 16 + 20 + + + + + 16777215 + 20 @@ -19289,7 +21094,13 @@ border-radius: 5; 145 - 16 + 20 + + + + + 16777215 + 20 @@ -19313,6 +21124,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>The bank angle where CruiseControl goes into inverted power mode. InvertedThrustReverse and InvertedPower control the direction and amount of power when in inverted mode.</p></body></html> @@ -19353,7 +21176,13 @@ border-radius: 5; 140 - 16 + 20 + + + + + 16777215 + 20 @@ -19377,6 +21206,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>Really just a safety limit. 3.0 means it will not use more than 3 times the power the throttle/collective stick is requesting.</p></body></html> @@ -19423,7 +21264,13 @@ border-radius: 5; 155 - 16 + 20 + + + + + 16777215 + 20 @@ -19456,7 +21303,13 @@ border-radius: 5; 90 - 16 + 20 + + + + + 16777215 + 20 @@ -19489,7 +21342,13 @@ border-radius: 5; 92 - 16 + 20 + + + + + 16777215 + 20 @@ -19529,6 +21388,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>If you find that quickly moving the stick around a lot makes the copter climb a bit, adjust this number down a little. It will be a compromise between climbing a little with lots of stick motion and descending a little with minimal stick motion.</p></body></html> @@ -19569,7 +21440,13 @@ border-radius: 5; 120 - 16 + 20 + + + + + 16777215 + 20 @@ -19602,7 +21479,13 @@ border-radius: 5; 97 - 16 + 20 + + + + + 16777215 + 20 @@ -19626,6 +21509,18 @@ border-radius: 5; + + + 0 + 22 + + + + + 16777215 + 22 + + <html><head/><body><p>Motor response time to go from min thrust to max thrust. It allows thrust anticipation on entering/exiting inverted mode</p></body></html> @@ -19696,22 +21591,6 @@ border-radius: 5; - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 9 - - - - @@ -22167,8 +24046,8 @@ font:bold; 0 0 - 815 - 708 + 817 + 699 @@ -25192,6 +27071,12 @@ Useful if you have accidentally changed some settings.
qtabbar.h
1 + + MixerCurve + QWidget +
mixercurve.h
+ 1 + stabilizationReloadBoardData_6 From e4d6171f2c46c1fc3bada360ba982259fea4f120 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sat, 20 Sep 2014 17:30:19 +0200 Subject: [PATCH 17/21] OP-1474 Moved TPS settings to PID banks. Rearranged GUI. --- .../config/configstabilizationwidget.cpp | 65 ++++++++++++++----- .../src/plugins/config/stabilization.ui | 26 ++++---- .../stabilizationsettings.xml | 4 -- 3 files changed, 62 insertions(+), 33 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 1e2258c0b..24a4f35d4 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -125,6 +125,9 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa connect(ui->advancedResponsivenessCheckBox, SIGNAL(toggled(bool)), this, SLOT(linkCheckBoxes(bool))); connect(ui->defaultThrottleCurveButton, SIGNAL(clicked()), this, SLOT(resetThrottleCurveToDefault())); + connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->ThrustPIDSource, SLOT(setEnabled(bool))); + connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->ThrustPIDTarget, SLOT(setEnabled(bool))); + connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->ThrustPIDAxis, SLOT(setEnabled(bool))); connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->thrustPIDScalingCurve, SLOT(setEnabled(bool))); ui->thrustPIDScalingCurve->setMixerType(MixerCurve::MIXERCURVE_TPA); ui->thrustPIDScalingCurve->setMin(-1); @@ -134,7 +137,6 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa addWidget(ui->enableThrustPIDScalingCheckBox); addWidget(ui->thrustPIDScalingCurve); addWidget(ui->thrustPIDScalingCurve->getCurveWidget()); - connect(this, SIGNAL(widgetContentsChanged(QWidget *)), this, SLOT(processLinkedWidgets(QWidget *))); connect(this, SIGNAL(autoPilotConnected()), this, SLOT(onBoardConnected())); @@ -166,45 +168,73 @@ void ConfigStabilizationWidget::updateObjectsFromWidgets() void ConfigStabilizationWidget::updateThrottleCurveFromObject() { - StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(stabBank); + qDebug() << "updatingCurveFromObject" << stabBank->getName(); - Q_ASSERT(stabSettings); + UAVObjectField *field = stabBank->getField("ThrustPIDScaleCurve"); + Q_ASSERT(field); QList curve; - for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { - curve.append(stabSettings->getThrustPIDScaleCurve(i)); + for (quint32 i = 0; i < field->getNumElements(); i++) { + qDebug() << field->getName() << field->getElementNames().at(i) << "=>" << field->getValue(i); + curve.append(field->getValue(i).toDouble()); } ui->thrustPIDScalingCurve->setCurve(&curve); - ui->enableThrustPIDScalingCheckBox->setChecked(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); - ui->thrustPIDScalingCurve->setEnabled(stabSettings->getEnableThrustPIDScaling() == StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE); + + field = stabBank->getField("EnableThrustPIDScaling"); + Q_ASSERT(field); + + bool enabled = field->getValue() == "TRUE"; + ui->enableThrustPIDScalingCheckBox->setChecked(enabled); + ui->thrustPIDScalingCurve->setEnabled(enabled); } void ConfigStabilizationWidget::updateObjectFromThrottleCurve() { - StabilizationSettings *stabSettings = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); + UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(stabBank); + qDebug() << "updatingObjectFromCurve" << stabBank->getName(); - Q_ASSERT(stabSettings); + UAVObjectField *field = stabBank->getField("ThrustPIDScaleCurve"); + Q_ASSERT(field); QList curve = ui->thrustPIDScalingCurve->getCurve(); - for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { - stabSettings->setThrustPIDScaleCurve(i, curve.at(i)); + for (quint32 i = 0; i < field->getNumElements(); i++) { + field->setValue(curve.at(i), i); + qDebug() << field->getName() << field->getElementNames().at(i) << "<=" << curve.at(i); } - stabSettings->setEnableThrustPIDScaling(ui->enableThrustPIDScalingCheckBox->isChecked() ? - StabilizationSettings::ENABLETHRUSTPIDSCALING_TRUE : StabilizationSettings::ENABLETHRUSTPIDSCALING_FALSE); + field = stabBank->getField("EnableThrustPIDScaling"); + Q_ASSERT(field); + field->setValue(ui->enableThrustPIDScalingCheckBox->isChecked() ? "TRUE" : "FALSE"); } void ConfigStabilizationWidget::resetThrottleCurveToDefault() { - StabilizationSettings defaultSettings; + UAVDataObject *defaultStabBank = (UAVDataObject*)getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(defaultStabBank); + defaultStabBank = defaultStabBank->dirtyClone(); + + UAVObjectField *field = defaultStabBank->getField("ThrustPIDScaleCurve"); + Q_ASSERT(field); QList curve; - for (quint32 i = 0; i < StabilizationSettings::THRUSTPIDSCALECURVE_NUMELEM; i++) { - curve.append(defaultSettings.getThrustPIDScaleCurve(i)); + for (quint32 i = 0; i < field->getNumElements(); i++) { + curve.append(field->getValue(i).toDouble()); } ui->thrustPIDScalingCurve->setCurve(&curve); + + field = defaultStabBank->getField("EnableThrustPIDScaling"); + Q_ASSERT(field); + + bool enabled = field->getValue() == "TRUE"; + ui->enableThrustPIDScalingCheckBox->setChecked(enabled); + ui->thrustPIDScalingCurve->setEnabled(enabled); + + delete defaultStabBank; } void ConfigStabilizationWidget::realtimeUpdatesSlot(bool value) @@ -300,6 +330,7 @@ void ConfigStabilizationWidget::onBoardConnected() void ConfigStabilizationWidget::pidBankChanged(int index) { + updateObjectFromThrottleCurve(); foreach(QTabBar * tabBar, m_pidTabBars) { disconnect(tabBar, SIGNAL(currentChanged(int)), this, SLOT(pidBankChanged(int))); tabBar->setCurrentIndex(index); @@ -313,6 +344,8 @@ void ConfigStabilizationWidget::pidBankChanged(int index) setWidgetBindingObjectEnabled(m_pidTabBars.at(0)->tabData(index).toString(), true); m_currentPIDBank = index; + qDebug() << "current bank:" << m_currentPIDBank; + updateThrottleCurveFromObject(); } bool ConfigStabilizationWidget::shouldObjectBeSaved(UAVObject *object) diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 26d274637..67368f799 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -6,8 +6,8 @@ 0 0 - 839 - 785 + 882 + 789 @@ -136,8 +136,8 @@ 0 0 - 801 - 726 + 860 + 703 @@ -7836,8 +7836,8 @@ border-radius: 5; 0 - -299 - 801 + 0 + 844 998 @@ -16207,7 +16207,7 @@ border-radius: 5; - + QFrame::NoFrame @@ -16836,7 +16836,7 @@ border-radius: 5; - + objname:StabilizationSettingsBankX fieldname:ThrustPIDScaleSource @@ -17961,7 +17961,7 @@ border-radius: 5; objname:StabilizationSettingsBankX - fieldname:ThrustPIDScaleAxis + fieldname:ThrustPIDScaleAxes buttongroup:99 @@ -18103,8 +18103,8 @@ border-radius: 5; 0 0 - 817 - 699 + 860 + 703 @@ -24046,8 +24046,8 @@ font:bold; 0 0 - 817 - 699 + 860 + 703 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index be805b737..a79798b4c 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -47,10 +47,6 @@ - - - - From 25cf7fcaf5d39f5a1ea556a02769b845905c1012 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sun, 21 Sep 2014 14:28:07 +0200 Subject: [PATCH 18/21] OP-1474 Added Axis labels to curves. --- .../configmultirotorwidget.cpp | 3 + .../config/cfg_vehicletypes/vehicleconfig.cpp | 2 +- .../config/configstabilizationwidget.cpp | 16 +- .../src/plugins/config/mixercurve.cpp | 18 +- .../src/plugins/config/mixercurve.h | 5 +- .../src/plugins/config/stabilization.ui | 17 +- .../uavobjectwidgetutils/mixercurvepoint.cpp | 66 +++---- .../uavobjectwidgetutils/mixercurvepoint.h | 42 +++-- .../uavobjectwidgetutils/mixercurvewidget.cpp | 162 ++++++++++++------ .../uavobjectwidgetutils/mixercurvewidget.h | 50 ++++-- 10 files changed, 239 insertions(+), 142 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index e1189d79d..662a25a4f 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -150,6 +150,9 @@ ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : // Connect the multirotor motor reverse checkbox connect(m_aircraft->MultirotorRevMixerCheckBox, SIGNAL(clicked(bool)), this, SLOT(reverseMultirotorMotor())); + + m_aircraft->multiThrottleCurve->setXAxisLabel(tr("Input")); + m_aircraft->multiThrottleCurve->setYAxisLabel(tr("Output")); updateEnableControls(); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index b6486e5cf..4f8ebe3d2 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -73,7 +73,7 @@ GUIConfigDataUnion VehicleConfig::getConfigData() } // sanity check - //Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); + // Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); return configData; } diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 24a4f35d4..9fc95ca3e 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -129,14 +129,15 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->ThrustPIDTarget, SLOT(setEnabled(bool))); connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->ThrustPIDAxis, SLOT(setEnabled(bool))); connect(ui->enableThrustPIDScalingCheckBox, SIGNAL(toggled(bool)), ui->thrustPIDScalingCurve, SLOT(setEnabled(bool))); - ui->thrustPIDScalingCurve->setMixerType(MixerCurve::MIXERCURVE_TPA); - ui->thrustPIDScalingCurve->setMin(-1); - ui->thrustPIDScalingCurve->setMax(1); + ui->thrustPIDScalingCurve->setXAxisLabel(tr("Thrust")); + ui->thrustPIDScalingCurve->setYAxisLabel(tr("Scaling factor")); + ui->thrustPIDScalingCurve->setMin(-0.5); + ui->thrustPIDScalingCurve->setMax(0.5); addWidget(ui->defaultThrottleCurveButton); addWidget(ui->enableThrustPIDScalingCheckBox); addWidget(ui->thrustPIDScalingCurve); - addWidget(ui->thrustPIDScalingCurve->getCurveWidget()); + addWidget(ui->thrustPIDScalingCurve); connect(this, SIGNAL(widgetContentsChanged(QWidget *)), this, SLOT(processLinkedWidgets(QWidget *))); connect(this, SIGNAL(autoPilotConnected()), this, SLOT(onBoardConnected())); @@ -169,6 +170,7 @@ void ConfigStabilizationWidget::updateObjectsFromWidgets() void ConfigStabilizationWidget::updateThrottleCurveFromObject() { UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(stabBank); qDebug() << "updatingCurveFromObject" << stabBank->getName(); @@ -194,13 +196,14 @@ void ConfigStabilizationWidget::updateThrottleCurveFromObject() void ConfigStabilizationWidget::updateObjectFromThrottleCurve() { UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(stabBank); qDebug() << "updatingObjectFromCurve" << stabBank->getName(); UAVObjectField *field = stabBank->getField("ThrustPIDScaleCurve"); Q_ASSERT(field); - QList curve = ui->thrustPIDScalingCurve->getCurve(); + QList curve = ui->thrustPIDScalingCurve->getCurve(); for (quint32 i = 0; i < field->getNumElements(); i++) { field->setValue(curve.at(i), i); qDebug() << field->getName() << field->getElementNames().at(i) << "<=" << curve.at(i); @@ -213,7 +216,8 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve() void ConfigStabilizationWidget::resetThrottleCurveToDefault() { - UAVDataObject *defaultStabBank = (UAVDataObject*)getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + UAVDataObject *defaultStabBank = (UAVDataObject *)getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString())); + Q_ASSERT(defaultStabBank); defaultStabBank = defaultStabBank->dirtyClone(); diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.cpp b/ground/openpilotgcs/src/plugins/config/mixercurve.cpp index 24bc0f88b..b2cc03a64 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.cpp +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.cpp @@ -96,14 +96,6 @@ void MixerCurve::setMixerType(MixerCurveType curveType) m_mixerUI->CurveMax->setMinimum(-1.0); break; } - case MixerCurve::MIXERCURVE_TPA: - { - m_mixerUI->SettingsGroup->setTitle("Thrust PID Scale"); - m_mixerUI->buttonGroup->hide(); - m_curve->setRange(-1.0, 1.0); - m_mixerUI->CurveMin->setMinimum(-1.0); - m_mixerUI->CurveMax->setMinimum(-1.0); - } default: break; } @@ -304,6 +296,16 @@ double MixerCurve::setRange(double min, double max) return m_curve->setRange(min, max); } +void MixerCurve::setXAxisLabel(QString label) +{ + m_curve->setXAxisLabel(label); +} + +void MixerCurve::setYAxisLabel(QString label) +{ + m_curve->setYAxisLabel(label); +} + double MixerCurve::getCurveMin() { return m_mixerUI->CurveMin->value(); diff --git a/ground/openpilotgcs/src/plugins/config/mixercurve.h b/ground/openpilotgcs/src/plugins/config/mixercurve.h index 7343043c9..faa581fcc 100644 --- a/ground/openpilotgcs/src/plugins/config/mixercurve.h +++ b/ground/openpilotgcs/src/plugins/config/mixercurve.h @@ -52,7 +52,7 @@ public: /* Enumeration options for ThrottleCurves */ - typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1, MIXERCURVE_TPA = 2 } MixerCurveType; + typedef enum { MIXERCURVE_THROTTLE = 0, MIXERCURVE_PITCH = 1 } MixerCurveType; void setMixerType(MixerCurveType curveType); void initCurve(const QList *points); @@ -68,6 +68,9 @@ public: double getCurveStep(); double setRange(double min, double max); + void setXAxisLabel(QString label); + void setYAxisLabel(QString label); + MixerCurveWidget *getCurveWidget() { return m_curve; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 67368f799..852d16d35 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -136,8 +136,8 @@ 0 0 - 860 - 703 + 844 + 726 @@ -7836,7 +7836,7 @@ border-radius: 5; 0 - 0 + -295 844 998 @@ -16192,7 +16192,7 @@ border-radius: 5; - + 0 @@ -16205,6 +16205,11 @@ border-radius: 5; 200 + + + 7 + + @@ -27072,9 +27077,9 @@ Useful if you have accidentally changed some settings. 1 - MixerCurve + MixerCurveWidget QWidget -
mixercurve.h
+
mixercurvewidget.h
1 diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp index 2a2dd2e1d..780d23f7d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp @@ -37,32 +37,33 @@ #include "mixercurvewidget.h" MixerNode::MixerNode(MixerCurveWidget *graphWidget) - : graph(graphWidget) + : m_graph(graphWidget) { setFlag(ItemIsMovable); setFlag(ItemSendsGeometryChanges); setCacheMode(DeviceCoordinateCache); setZValue(-1); - vertical = false; - drawNode = true; - drawText = true; + m_vertical = false; + m_drawNode = true; + m_drawText = true; - positiveColor = "#609FF2"; // blueish? - neutralColor = "#14CE24"; // greenish? - negativeColor = "#EF5F5F"; // redish? - disabledColor = "#dddddd"; - disabledTextColor = "#aaaaaa"; + m_alpha = 0.7; + m_positiveColor = "#609FF2"; // blueish? + m_neutralColor = "#14CE24"; // greenish? + m_negativeColor = "#EF5F5F"; // redish? + m_disabledColor = "#dddddd"; + m_disabledTextColor = "#aaaaaa"; } void MixerNode::addEdge(Edge *edge) { - edgeList << edge; + m_edgeList << edge; edge->adjust(); } QList MixerNode::edges() const { - return edgeList; + return m_edgeList; } @@ -83,18 +84,19 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, { QString text = QString().sprintf("%.2f", value()); - painter->setFont(graph->font()); - if (drawNode) { + painter->setFont(m_graph->font()); + if (m_drawNode) { QRadialGradient gradient(-3, -3, 10); QColor color; if (value() < 0) { - color = negativeColor; + color = m_negativeColor; } else if (value() == 0) { - color = neutralColor; + color = m_neutralColor; } else { - color = positiveColor; + color = m_positiveColor; } + color.setAlphaF(m_alpha); if (option->state & QStyle::State_Sunken) { gradient.setCenter(3, 3); @@ -104,23 +106,23 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, gradient.setColorAt(1, selColor.darker()); gradient.setColorAt(0, selColor); } else { - gradient.setColorAt(0, graph->isEnabled() ? color : disabledColor); - gradient.setColorAt(1, graph->isEnabled() ? color.darker() : disabledColor); + gradient.setColorAt(0, m_graph->isEnabled() ? color : m_disabledColor); + gradient.setColorAt(1, m_graph->isEnabled() ? color.darker() : m_disabledColor); } painter->setBrush(gradient); - painter->setPen(graph->isEnabled() ? QPen(Qt::black, 0) : QPen(disabledTextColor)); + painter->setPen(m_graph->isEnabled() ? QPen(Qt::black, 0) : QPen(m_disabledTextColor)); painter->drawEllipse(boundingRect()); - if (!image.isNull()) { - painter->drawImage(boundingRect().adjusted(1, 1, -1, -1), image); + if (!m_image.isNull()) { + painter->drawImage(boundingRect().adjusted(1, 1, -1, -1), m_image); } } - if (drawText) { - if (graph->isEnabled()) { - painter->setPen(QPen(drawNode ? Qt::white : Qt::black, 0)); + if (m_drawText) { + if (m_graph->isEnabled()) { + painter->setPen(QPen(m_drawNode ? Qt::white : Qt::black, 0)); } else { - painter->setPen(QPen(disabledTextColor)); + painter->setPen(QPen(m_disabledTextColor)); } painter->drawText((value() < 0) ? -10 : -8, 3, text); @@ -129,27 +131,27 @@ void MixerNode::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, void MixerNode::verticalMove(bool flag) { - vertical = flag; + m_vertical = flag; } double MixerNode::value() { - double h = graph->sceneRect().height(); + double h = m_graph->sceneRect().height(); double ratio = (h - pos().y()) / h; - return ((graph->getMax() - graph->getMin()) * ratio) + graph->getMin(); + return ((m_graph->getMax() - m_graph->getMin()) * ratio) + m_graph->getMin(); } QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) { QPointF newPos = val.toPointF(); - double h = graph->sceneRect().height(); + double h = m_graph->sceneRect().height(); switch (change) { case ItemPositionChange: { - if (!vertical) { + if (!m_vertical) { break; } @@ -169,12 +171,12 @@ QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) } case ItemPositionHasChanged: { - foreach(Edge * edge, edgeList) + foreach(Edge * edge, m_edgeList) edge->adjust(); update(); - graph->itemMoved(value()); + m_graph->itemMoved(value()); break; } default: diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h index 27ad9eb11..7923d50b9 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h @@ -59,27 +59,32 @@ public: void setPositiveColor(QColor color = "#609FF2") { - positiveColor = color; + m_positiveColor = color; } void setNegativeColor(QColor color = "#EF5F5F") { - negativeColor = color; + m_negativeColor = color; + } + + void setAlpha(qreal alpha) + { + m_alpha = alpha; } void setImage(QImage img) { - image = img; + m_image = img; } void setDrawNode(bool draw) { - drawNode = draw; + m_drawNode = draw; } void setDrawText(bool draw) { - drawText = draw; + m_drawText = draw; } QRectF boundingRect() const; @@ -95,22 +100,23 @@ protected: void mouseReleaseEvent(QGraphicsSceneMouseEvent *event); private: - QList edgeList; - QPointF newPos; - MixerCurveWidget *graph; + QList m_edgeList; + QPointF m_newPos; + MixerCurveWidget *m_graph; - QColor positiveColor; - QColor neutralColor; - QColor negativeColor; - QColor disabledColor; - QColor disabledTextColor; + qreal m_alpha; + QColor m_positiveColor; + QColor m_neutralColor; + QColor m_negativeColor; + QColor m_disabledColor; + QColor m_disabledTextColor; - QImage image; + QImage m_image; - bool vertical; - bool drawNode; - bool drawText; - int index; + bool m_vertical; + bool m_drawNode; + bool m_drawText; + int m_index; }; #endif // MIXERCURVEPOINT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp index 37d9f3be5..067498b92 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp @@ -36,7 +36,8 @@ /* * Initialize the widget */ -MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) +MixerCurveWidget::MixerCurveWidget(QWidget *parent) : + QGraphicsView(parent), m_xAxisTextItem(0), m_yAxisTextItem(0) { // Create a layout, add a QGraphicsView and put the SVG inside. // The Mixer Curve widget looks like this: @@ -54,50 +55,60 @@ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : QGraphicsView(parent) setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); setRenderHint(QPainter::Antialiasing); - curveMin = 0.0; - curveMax = 1.0; + m_curveMin = 0.0; + m_curveMax = 1.0; setFrameStyle(QFrame::NoFrame); setStyleSheet("background:transparent"); QGraphicsScene *scene = new QGraphicsScene(this); QSvgRenderer *renderer = new QSvgRenderer(); - plot = new QGraphicsSvgItem(); + m_plot = new QGraphicsSvgItem(); renderer->load(QString(":/uavobjectwidgetutils/images/curve-bg.svg")); - plot->setSharedRenderer(renderer); + m_plot->setSharedRenderer(renderer); - scene->addItem(plot); - plot->setZValue(-1); + scene->addItem(m_plot); + m_plot->setZValue(-1); - scene->setSceneRect(plot->boundingRect()); + scene->setSceneRect(m_plot->boundingRect()); setScene(scene); + setupXAxisLabel(); + setupYAxisLabel(); initNodes(MixerCurveWidget::NODE_NUMELEM); } MixerCurveWidget::~MixerCurveWidget() { - while (!nodeList.isEmpty()) { - delete nodeList.takeFirst(); + while (!m_nodeList.isEmpty()) { + delete m_nodeList.takeFirst(); } - while (!edgeList.isEmpty()) { - delete edgeList.takeFirst(); + while (!m_edgeList.isEmpty()) { + delete m_edgeList.takeFirst(); + } + if (m_xAxisTextItem) { + delete m_xAxisTextItem; + m_xAxisTextItem = NULL; + } + if (m_yAxisTextItem) { + delete m_yAxisTextItem; + m_yAxisTextItem = NULL; } } void MixerCurveWidget::setPositiveColor(QString color) { - for (int i = 0; i < nodeList.count(); i++) { - MixerNode *node = nodeList.at(i); + for (int i = 0; i < m_nodeList.count(); i++) { + MixerNode *node = m_nodeList.at(i); node->setPositiveColor(color); } } void MixerCurveWidget::setNegativeColor(QString color) { - for (int i = 0; i < nodeList.count(); i++) { - MixerNode *node = nodeList.at(i); + for (int i = 0; i < m_nodeList.count(); i++) { + MixerNode *node = m_nodeList.at(i); node->setNegativeColor(color); } } @@ -120,8 +131,8 @@ void MixerCurveWidget::initCurve(const QList *points) void MixerCurveWidget::initNodes(int numPoints) { // First of all, clear any existing list - if (nodeList.count()) { - foreach(MixerNode * node, nodeList) { + if (m_nodeList.count()) { + foreach(MixerNode * node, m_nodeList) { foreach(Edge * edge, node->edges()) { if (edge->sourceNode() == node) { scene()->removeItem(edge); @@ -132,7 +143,7 @@ void MixerCurveWidget::initNodes(int numPoints) delete node; } - nodeList.clear(); + m_nodeList.clear(); } // Create the nodes and edges @@ -140,7 +151,7 @@ void MixerCurveWidget::initNodes(int numPoints) for (int i = 0; i < numPoints; i++) { MixerNode *node = new MixerNode(this); - nodeList.append(node); + m_nodeList.append(node); scene()->addItem(node); node->setPos(0, 0); @@ -153,6 +164,32 @@ void MixerCurveWidget::initNodes(int numPoints) } } +void MixerCurveWidget::setupXAxisLabel() +{ + if (!m_xAxisString.isEmpty()) { + if (m_xAxisTextItem) { + m_xAxisTextItem->setPlainText(m_xAxisString); + } else { + m_xAxisTextItem = new QGraphicsTextItem(m_xAxisString, m_plot); + scene()->addItem(m_xAxisTextItem); + } + } +} + +void MixerCurveWidget::setupYAxisLabel() +{ + if (!m_yAxisString.isEmpty()) { + if (m_yAxisTextItem) { + m_yAxisTextItem->setPlainText(m_yAxisString); + } else { + m_yAxisTextItem = new QGraphicsTextItem(m_yAxisString, m_plot); + // m_yAxisTextItem->setTransformOriginPoint(m_yAxisTextItem->boundingRect().height(), m_yAxisTextItem->boundingRect().left()); + m_yAxisTextItem->setRotation(270); + scene()->addItem(m_yAxisTextItem); + } + } +} + /** Returns the current curve settings */ @@ -160,7 +197,7 @@ QList MixerCurveWidget::getCurve() { QList list; - foreach(MixerNode * node, nodeList) { + foreach(MixerNode * node, m_nodeList) { list.append(node->value()); } @@ -185,55 +222,51 @@ void MixerCurveWidget::initLinearCurve(int numPoints, double maxValue, double mi */ void MixerCurveWidget::setCurve(const QList *points) { - curveUpdating = true; + m_curveUpdating = true; int ptCnt = points->count(); - if (nodeList.count() != ptCnt) { + if (m_nodeList.count() != ptCnt) { initNodes(ptCnt); } - double range = curveMax - curveMin; + double range = m_curveMax - m_curveMin; - qreal w = plot->boundingRect().width() / (ptCnt - 1); - qreal h = plot->boundingRect().height(); + qreal w = m_plot->boundingRect().width() / (ptCnt - 1); + qreal h = m_plot->boundingRect().height(); for (int i = 0; i < ptCnt; i++) { - double val = (points->at(i) < curveMin) ? curveMin : (points->at(i) > curveMax) ? curveMax : points->at(i); + double val = (points->at(i) < m_curveMin) ? m_curveMin : (points->at(i) > m_curveMax) ? m_curveMax : points->at(i); val += range; - val -= (curveMin + range); + val -= (m_curveMin + range); val /= range; - MixerNode *node = nodeList.at(i); + MixerNode *node = m_nodeList.at(i); node->setPos(w * i, h - (val * h)); node->verticalMove(true); node->update(); } - curveUpdating = false; + m_curveUpdating = false; update(); emit curveUpdated(); } - void MixerCurveWidget::showEvent(QShowEvent *event) { Q_UNUSED(event) - // Thit fitInView method should only be called now, once the - // widget is shown, otherwise it cannot compute its values and - // the result is usually a ahrsbargraph that is way too small. - - QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-12, -12, 12, 12), Qt::KeepAspectRatio); + positionAxisLabels(); + setSceneRect(scene()->itemsBoundingRect()); + fitInView(scene()->itemsBoundingRect(), Qt::KeepAspectRatio); } void MixerCurveWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - - QRectF rect = plot->boundingRect(); - fitInView(rect.adjusted(-12, -12, 12, 12), Qt::KeepAspectRatio); + positionAxisLabels(); + setSceneRect(scene()->itemsBoundingRect()); + fitInView(scene()->itemsBoundingRect(), Qt::KeepAspectRatio); } void MixerCurveWidget::changeEvent(QEvent *event) @@ -241,50 +274,77 @@ void MixerCurveWidget::changeEvent(QEvent *event) QGraphicsView::changeEvent(event); if (event->type() == QEvent::EnabledChange) { - foreach(MixerNode * node, nodeList) { + foreach(MixerNode * node, m_nodeList) { node->update(); } } } +void MixerCurveWidget::positionAxisLabels() +{ + QRectF rect = m_plot->boundingRect(); + + if (m_xAxisTextItem) { + m_xAxisTextItem->setPos(rect.right() - + m_xAxisTextItem->boundingRect().width(), rect.bottom() - 4); + } + + if (m_yAxisTextItem) { + m_yAxisTextItem->setPos(rect.left() - + m_yAxisTextItem->boundingRect().height(), m_yAxisTextItem->boundingRect().width()); + } +} + void MixerCurveWidget::itemMoved(double itemValue) { Q_UNUSED(itemValue); - if (!curveUpdating) { + if (!m_curveUpdating) { emit curveUpdated(); } } void MixerCurveWidget::setMin(double value) { - if (curveMin != value) { + if (m_curveMin != value) { emit curveMinChanged(value); } - curveMin = value; + m_curveMin = value; } void MixerCurveWidget::setMax(double value) { - if (curveMax != value) { + if (m_curveMax != value) { emit curveMaxChanged(value); } - curveMax = value; + m_curveMax = value; } double MixerCurveWidget::getMin() { - return curveMin; + return m_curveMin; } double MixerCurveWidget::getMax() { - return curveMax; + return m_curveMax; } double MixerCurveWidget::setRange(double min, double max) { - curveMin = min; - curveMax = max; - return curveMax - curveMin; + m_curveMin = min; + m_curveMax = max; + return m_curveMax - m_curveMin; +} + +void MixerCurveWidget::setXAxisLabel(QString label) +{ + m_xAxisString = label; + setupXAxisLabel(); +} + +void MixerCurveWidget::setYAxisLabel(QString label) +{ + m_yAxisString = label; + setupYAxisLabel(); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.h index 8cc64d8b5..24ff448a7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.h @@ -41,6 +41,8 @@ class UAVOBJECTWIDGETUTILS_EXPORT MixerCurveWidget : public QGraphicsView { Q_OBJECT public: + static const int NODE_NUMELEM = 5; + MixerCurveWidget(QWidget *parent = 0); ~MixerCurveWidget(); @@ -57,34 +59,44 @@ public: double getMax(); double setRange(double min, double max); - static const int NODE_NUMELEM = 5; +public slots: + void setXAxisLabel(QString label); + void setYAxisLabel(QString label); signals: void curveUpdated(); void curveMinChanged(double value); void curveMaxChanged(double value); -private slots: - -private: - QGraphicsSvgItem *plot; - - QList edgeList; - QList nodeList; - - double curveMin; - double curveMax; - bool curveUpdating; - - void initNodes(int numPoints); - void setPositiveColor(QString color); - void setNegativeColor(QString color); - - void resizeCommands(); - protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); void changeEvent(QEvent *event); + +private slots: + void positionAxisLabels(); + +private: + QGraphicsSvgItem *m_plot; + QGraphicsTextItem *m_xAxisTextItem; + QGraphicsTextItem *m_yAxisTextItem; + + QList m_edgeList; + QList m_nodeList; + + QString m_xAxisString; + QString m_yAxisString; + + double m_curveMin; + double m_curveMax; + bool m_curveUpdating; + + void initNodes(int numPoints); + void setupXAxisLabel(); + void setupYAxisLabel(); + void setPositiveColor(QString color); + void setNegativeColor(QString color); + + void resizeCommands(); }; #endif /* MIXERCURVEWIDGET_H_ */ From 9d42430ee7c29f0519c302ad54d5a6b820bef5e4 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sun, 21 Sep 2014 22:10:23 +0200 Subject: [PATCH 19/21] OP-1474 Fixed a sizing/value calculation issue. --- .../src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp | 8 ++++---- .../src/plugins/uavobjectwidgetutils/mixercurvepoint.h | 3 ++- .../src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp | 5 ++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp index 780d23f7d..533eabcd7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp @@ -36,8 +36,8 @@ #include "mixercurvepoint.h" #include "mixercurvewidget.h" -MixerNode::MixerNode(MixerCurveWidget *graphWidget) - : m_graph(graphWidget) +MixerNode::MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem* graphItem) + : m_graph(graphWidget), m_graphItem(graphItem) { setFlag(ItemIsMovable); setFlag(ItemSendsGeometryChanges); @@ -136,7 +136,7 @@ void MixerNode::verticalMove(bool flag) double MixerNode::value() { - double h = m_graph->sceneRect().height(); + double h = m_graphItem->boundingRect().height(); double ratio = (h - pos().y()) / h; return ((m_graph->getMax() - m_graph->getMin()) * ratio) + m_graph->getMin(); @@ -146,7 +146,7 @@ double MixerNode::value() QVariant MixerNode::itemChange(GraphicsItemChange change, const QVariant &val) { QPointF newPos = val.toPointF(); - double h = m_graph->sceneRect().height(); + double h = m_graphItem->boundingRect().height(); switch (change) { case ItemPositionChange: diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h index 7923d50b9..899070871 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h @@ -45,7 +45,7 @@ class UAVOBJECTWIDGETUTILS_EXPORT MixerNode : public QObject, public QGraphicsIt Q_OBJECT Q_INTERFACES(QGraphicsItem) public: - MixerNode(MixerCurveWidget *graphWidget); + MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem* graphItem); void addEdge(Edge *edge); QList edges() const; @@ -103,6 +103,7 @@ private: QList m_edgeList; QPointF m_newPos; MixerCurveWidget *m_graph; + QGraphicsItem* m_graphItem; qreal m_alpha; QColor m_positiveColor; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp index 067498b92..d6c7f533a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvewidget.cpp @@ -60,7 +60,7 @@ MixerCurveWidget::MixerCurveWidget(QWidget *parent) : setFrameStyle(QFrame::NoFrame); setStyleSheet("background:transparent"); - + setRenderHint(QPainter::HighQualityAntialiasing, true); QGraphicsScene *scene = new QGraphicsScene(this); QSvgRenderer *renderer = new QSvgRenderer(); m_plot = new QGraphicsSvgItem(); @@ -149,7 +149,7 @@ void MixerCurveWidget::initNodes(int numPoints) // Create the nodes and edges MixerNode *prevNode = 0; for (int i = 0; i < numPoints; i++) { - MixerNode *node = new MixerNode(this); + MixerNode *node = new MixerNode(this, m_plot); m_nodeList.append(node); scene()->addItem(node); @@ -183,7 +183,6 @@ void MixerCurveWidget::setupYAxisLabel() m_yAxisTextItem->setPlainText(m_yAxisString); } else { m_yAxisTextItem = new QGraphicsTextItem(m_yAxisString, m_plot); - // m_yAxisTextItem->setTransformOriginPoint(m_yAxisTextItem->boundingRect().height(), m_yAxisTextItem->boundingRect().left()); m_yAxisTextItem->setRotation(270); scene()->addItem(m_yAxisTextItem); } From 508112d5a33ec3a71d02701859415d69cb5acc14 Mon Sep 17 00:00:00 2001 From: m_thread Date: Sun, 21 Sep 2014 22:38:49 +0200 Subject: [PATCH 20/21] OP-1474 Fixed code asserting in debug. --- .../src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp | 2 +- .../src/plugins/config/cfg_vehicletypes/vehicleconfig.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index 4f8ebe3d2..1ebfba307 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -73,7 +73,7 @@ GUIConfigDataUnion VehicleConfig::getConfigData() } // sanity check - // Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); + Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == (sizeof(configData.UAVObject) / sizeof(configData.UAVObject[0]))); return configData; } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h index 26e4963bd..6dfe1760a 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h @@ -131,7 +131,7 @@ typedef struct { typedef union { - uint UAVObject[5]; // 32 bits * 5 + uint UAVObject[4]; // 32 bits * 4 heliGUISettingsStruct heli; // 128 bits fixedGUISettingsStruct fixedwing; multiGUISettingsStruct multi; From 9166363f09bf6313995a74357c9f971d08826bad Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 22 Sep 2014 22:05:20 +0200 Subject: [PATCH 21/21] OP-1474 Uncrustify --- ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp | 3 ++- .../src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp | 2 +- .../src/plugins/uavobjectwidgetutils/mixercurvepoint.h | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 229eb608c..f85cf72c1 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -45,7 +45,7 @@ void ModelUavoProxy::sendPathPlan() { modelToObjects(); - PathPlan *pathPlan = PathPlan::GetInstance(objMngr); + PathPlan *pathPlan = PathPlan::GetInstance(objMngr); const int waypointCount = pathPlan->getWaypointCount(); const int actionCount = pathPlan->getPathActionCount(); @@ -98,6 +98,7 @@ void ModelUavoProxy::sendPathPlan() void ModelUavoProxy::receivePathPlan() { QProgressDialog progress(tr("Receiving the path plan from the board... "), "", 0, 0); + progress.setWindowModality(Qt::WindowModal); progress.setCancelButton(NULL); progress.show(); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp index 533eabcd7..2ab2c2478 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.cpp @@ -36,7 +36,7 @@ #include "mixercurvepoint.h" #include "mixercurvewidget.h" -MixerNode::MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem* graphItem) +MixerNode::MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem *graphItem) : m_graph(graphWidget), m_graphItem(graphItem) { setFlag(ItemIsMovable); diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h index 899070871..1b6c74dcf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/mixercurvepoint.h @@ -45,7 +45,7 @@ class UAVOBJECTWIDGETUTILS_EXPORT MixerNode : public QObject, public QGraphicsIt Q_OBJECT Q_INTERFACES(QGraphicsItem) public: - MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem* graphItem); + MixerNode(MixerCurveWidget *graphWidget, QGraphicsItem *graphItem); void addEdge(Edge *edge); QList edges() const; @@ -103,7 +103,7 @@ private: QList m_edgeList; QPointF m_newPos; MixerCurveWidget *m_graph; - QGraphicsItem* m_graphItem; + QGraphicsItem *m_graphItem; qreal m_alpha; QColor m_positiveColor;