diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc index c4790afd6bef79e5814c7f3b444130fb347aaa2f..6a953ca23c9ef62138760e231653795c0512305a 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc @@ -105,23 +105,39 @@ void AirframeComponentController::changeAutostart(void) qgcApp()->setOverrideCursor(Qt::WaitCursor); - getParameterFact(-1, "SYS_AUTOSTART")->setValue(_autostartId); - getParameterFact(-1, "SYS_AUTOCONFIG")->setValue(1); + Fact* sysAutoStartFact = getParameterFact(-1, "SYS_AUTOSTART"); + Fact* sysAutoConfigFact = getParameterFact(-1, "SYS_AUTOCONFIG"); - // FactSystem doesn't currently have a mechanism to wait for the parameters to come backf from the board. - // So instead we wait for enough time for the parameters to hoepfully make it to the board. - qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); - QGC::SLEEP::sleep(3); - qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); + // We need to wait for the vehicleUpdated signals to come back before we reboot + _waitParamWriteSignalCount = 0; + connect(sysAutoStartFact, &Fact::vehicleUpdated, this, &AirframeComponentController::_waitParamWriteSignal); + connect(sysAutoConfigFact, &Fact::vehicleUpdated, this, &AirframeComponentController::_waitParamWriteSignal); - // Reboot board + // We use forceSetValue to params are sent even if the previous value is that same as the new value + sysAutoStartFact->forceSetValue(_autostartId); + sysAutoConfigFact->forceSetValue(1); +} + +void AirframeComponentController::_waitParamWriteSignal(QVariant value) +{ + Q_UNUSED(value); + _waitParamWriteSignalCount++; + if (_waitParamWriteSignalCount == 2) { + // Now that both params have made it to the vehicle we can reboot it. All these signals are flying + // around on the main thread, so we need to allow the stack to unwind back to the event loop before + // we reboot. + QTimer::singleShot(100, this, &AirframeComponentController::_rebootAfterStackUnwind); + } +} + +void AirframeComponentController::_rebootAfterStackUnwind(void) +{ _uas->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); QGC::SLEEP::sleep(1); qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); LinkManager::instance()->disconnectAll(); - qgcApp()->restoreOverrideCursor(); } diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentController.h b/src/AutoPilotPlugins/PX4/AirframeComponentController.h index 8a3f1a50be086a14ccf4af94033b8dffceef0f8d..1e4f84780f81981904976a54622d716ea9143b2d 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentController.h +++ b/src/AutoPilotPlugins/PX4/AirframeComponentController.h @@ -63,15 +63,20 @@ signals: void autostartIdChanged(int newAutostartId); void showCustomConfigPanelChanged(bool show); +private slots: + void _waitParamWriteSignal(QVariant value); + void _rebootAfterStackUnwind(void); + private: static bool _typesRegistered; - QVariantList _airframeTypes; - QString _currentAirframeType; - QString _currentVehicleName; - int _currentVehicleIndex; - int _autostartId; - bool _showCustomConfigPanel; + QVariantList _airframeTypes; + QString _currentAirframeType; + QString _currentVehicleName; + int _currentVehicleIndex; + int _autostartId; + bool _showCustomConfigPanel; + int _waitParamWriteSignalCount; }; class Airframe : public QObject diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 54e33dc906239547c0a0a3a7d8f69f72fb18ed61..6ae59e82bdff86d98cf1582d3af11ac005e93a4f 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -49,6 +49,22 @@ Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObjec } +void Fact::forceSetValue(const QVariant& value) +{ + if (_metaData) { + QVariant typedValue; + QString errorString; + + if (_metaData->convertAndValidate(value, true /* convertOnly */, typedValue, errorString)) { + _value.setValue(typedValue); + emit valueChanged(_value); + emit _containerValueChanged(_value); + } + } else { + qWarning() << "Meta data pointer missing"; + } +} + void Fact::setValue(const QVariant& value) { if (_metaData) { @@ -71,6 +87,7 @@ void Fact::_containerSetValue(const QVariant& value) { _value = value; emit valueChanged(_value); + emit vehicleUpdated(_value); } QString Fact::name(void) const diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index bcb1d298936603627ea06d65aa007dccb1b7362e..dad179ac675459df5a6bae4b1de37850c90e9458 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -84,6 +84,9 @@ public: bool maxIsDefaultForType(void); QString group(void); + /// Sets and sends new value to vehicle even if value is the same + void forceSetValue(const QVariant& value); + /// Sets the meta data associated with the Fact. void setMetaData(FactMetaData* metaData); @@ -95,6 +98,9 @@ signals: /// This signal is only meant for use by the QT property system. It should not be connected to by client code. void valueChanged(QVariant value); + /// Signalled when the param write ack comes back from the vehicle + void vehicleUpdated(QVariant value); + /// Signalled when property has been changed by a call to the property write accessor /// /// This signal is meant for use by Fact container implementations.