diff --git a/ground/src/plugins/config/airframe.ui b/ground/src/plugins/config/airframe.ui index 91a95e395..8b4c1fbcb 100644 --- a/ground/src/plugins/config/airframe.ui +++ b/ground/src/plugins/config/airframe.ui @@ -718,12 +718,19 @@ Typical value is 50% for + or X configuration on quads. true - When tuning: Slowly raise accel time from zero to just + In miliseconds. +When tuning: Slowly raise accel time from zero to just under the level where the motor starts to overshoot its target speed. + + 3 + - 1000.000000000000000 + 100.000000000000000 + + + 0.010000000000000 @@ -746,8 +753,14 @@ its target speed when decelerating. Do it after accel time is setup. + + 3 + - 1000.000000000000000 + 100.000000000000000 + + + 0.010000000000000 @@ -794,14 +807,17 @@ In 'units per second', a sound default is 1000. - - - false + + + Qt::Horizontal - - Select the test channel for tuning feed forward. + + + 40 + 20 + - + @@ -812,7 +828,7 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Sans'; font-size:10pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware! Check <span style=" font-weight:600;">all three</span> checkboxes to test the</p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">selected channel.</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run even if your airframe is not armed.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run only if your airframe armed.</p></body></html> @@ -828,7 +844,7 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Sans'; font-size:10pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware! Check <span style=" font-weight:600;">all three</span> checkboxes to test the</p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">selected channel.</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run even if your airframe is not armed.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run only if your airframe armed.</p></body></html> @@ -844,7 +860,7 @@ p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'Sans'; font-size:10pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware! Check <span style=" font-weight:600;">all three</span> checkboxes to test the</p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">selected channel.</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run even if your airframe is not armed.</p></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It will run only if your airframe armed.</p></body></html> Enable FF tuning diff --git a/ground/src/plugins/config/configairframewidget.cpp b/ground/src/plugins/config/configairframewidget.cpp index ccfee6de9..970cf1aca 100644 --- a/ground/src/plugins/config/configairframewidget.cpp +++ b/ground/src/plugins/config/configairframewidget.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,9 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p m_aircraft = new Ui_AircraftWidget(); m_aircraft->setupUi(this); + ffTuningInProgress = false; + ffTuningPhase = false; + mixerTypes << "Mixer0Type" << "Mixer1Type" << "Mixer2Type" << "Mixer3Type" << "Mixer4Type" << "Mixer5Type" << "Mixer6Type" << "Mixer7Type"; mixerVectors << "Mixer0Vector" << "Mixer1Vector" << "Mixer2Vector" << "Mixer3Vector" @@ -110,6 +114,11 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p // connect(m_aircraft->fwAileron1Channel, SIGNAL(currentIndexChanged(int)), this, SLOT(toggleAileron2(int))); // connect(m_aircraft->fwElevator1Channel, SIGNAL(currentIndexChanged(int)), this, SLOT(toggleElevator2(int))); + // Now connect the three feed forward test checkboxes + connect(m_aircraft->ffTestBox1, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); + connect(m_aircraft->ffTestBox2, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); + connect(m_aircraft->ffTestBox3, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); + connect(parent, SIGNAL(autopilotConnected()),this, SLOT(requestAircraftUpdate())); } @@ -128,8 +137,6 @@ void ConfigAirframeWidget::switchAirframeType(int index){ m_aircraft->airframesWidget->setCurrentIndex(index); m_aircraft->quadShape->setSceneRect(quad->boundingRect()); m_aircraft->quadShape->fitInView(quad, Qt::KeepAspectRatio); - - } void ConfigAirframeWidget::showEvent(QShowEvent *event) @@ -145,7 +152,6 @@ void ConfigAirframeWidget::resizeEvent(QResizeEvent* event) { Q_UNUSED(event); m_aircraft->quadShape->fitInView(quad, Qt::KeepAspectRatio); - } @@ -171,6 +177,80 @@ void ConfigAirframeWidget::toggleElevator2(int index) } } +///////////////////////////////////////////////////////// +/// Feed Forward Testing +///////////////////////////////////////////////////////// + +/** + Enables and runs feed forward testing + */ +void ConfigAirframeWidget::enableFFTest() +{ + // Role: + // - Check if all three checkboxes are checked + // - Every other timer event: toggle engine from 1/3 to 2/3 + // - Every other time event: send FF settings to flight FW + if (m_aircraft->ffTestBox1->isChecked() && + m_aircraft->ffTestBox2->isChecked() && + m_aircraft->ffTestBox3->isChecked()) { + if (!ffTuningInProgress) + { + // Initiate tuning: + // Setup a special mixer with all channels disabled but one + // and no RPY dependency. + UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); + accInitialData = mdata; + mdata.flightAccess = UAVObject::ACCESS_READONLY; +/* + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; + mdata.gcsTelemetryAcked = false; + mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + // NOTE: if actuators not updated at least every 100ms, then the + // flight sw goes into failsafe, hence this very low update period: + mdata.gcsTelemetryUpdatePeriod = 25; + */ + obj->setMetadata(mdata); + } + // Depending on phase, either move actuator or send FF settings: + if (ffTuningPhase) { + // Send FF settings to the board + // We can already setup the feedforward here, as it is common to all platforms + UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + UAVObjectField* field = obj->getField(QString("FeedForward")); + field->setDouble((double)m_aircraft->feedForwardSlider->value()/100); + field = obj->getField(QString("AccelTime")); + field->setDouble(m_aircraft->accelTime->value()); + field = obj->getField(QString("DecelTime")); + field->setDouble(m_aircraft->decelTime->value()); + field = obj->getField(QString("MaxAccel")); + field->setDouble(m_aircraft->maxAccelSlider->value()); + obj->updated(); + } else { + // Toggle motor state + UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlCommand"))); + double value = obj->getField("Throttle")->getDouble(); + double target = (value < 0.5) ? 0.55 : 0.45; + obj->getField("Throttle")->setValue(target); + obj->updated(); + } + ffTuningPhase = !ffTuningPhase; + ffTuningInProgress = true; + QTimer::singleShot(1000, this, SLOT(enableFFTest())); + } else { + // - If no: disarm timer, restore actuatorcommand metadata + // Disarm! + if (ffTuningInProgress) { + ffTuningInProgress = false; + UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlCommand"))); + UAVObject::Metadata mdata = obj->getMetadata(); + mdata = accInitialData; // Restore metadata + obj->setMetadata(mdata); + } + } +} + + /** Resets Fixed wing throttle mixer */ diff --git a/ground/src/plugins/config/configairframewidget.h b/ground/src/plugins/config/configairframewidget.h index 05faa5821..c388686fb 100644 --- a/ground/src/plugins/config/configairframewidget.h +++ b/ground/src/plugins/config/configairframewidget.h @@ -63,6 +63,9 @@ private: QStringList mixerTypes; QStringList mixerVectors; QGraphicsSvgItem *quad; + bool ffTuningInProgress; + bool ffTuningPhase; + UAVObject::Metadata accInitialData; private slots: void requestAircraftUpdate(); @@ -76,6 +79,7 @@ private slots: void resetMrMixer(); void updateFwThrottleCurveValue(QList list, double value); void updateMrThrottleCurveValue(QList list, double value); + void enableFFTest(); protected: void showEvent(QShowEvent *event);