Merge pull request #1755 from DonLakeFlyer/AirConfig

Wait for params before reboot

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

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

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

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.