From 69d38e40041d7ff549578a1d37c1a8359f4dfa8f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Aug 2015 19:24:13 +0200
Subject: [PATCH] Add support for new bus config message

---
 src/AutoPilotPlugins/PX4/PowerComponent.qml   | 31 +++++++++++++++++
 .../PX4/PowerComponentController.cc           | 34 ++++++++++++++++++-
 .../PX4/PowerComponentController.h            |  2 ++
 3 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/src/AutoPilotPlugins/PX4/PowerComponent.qml b/src/AutoPilotPlugins/PX4/PowerComponent.qml
index e83fc04e8..1db5b8aea 100644
--- a/src/AutoPilotPlugins/PX4/PowerComponent.qml
+++ b/src/AutoPilotPlugins/PX4/PowerComponent.qml
@@ -263,6 +263,37 @@ QGCView {
                 }
             }
 
+            QGCLabel {
+                text:           "UAVCAN ESC Configuration"
+                font.pixelSize: ScreenTools.mediumFontPixelSize
+            }
+
+            Rectangle {
+                width:              parent.width
+                height:             140
+                color:              palette.windowShade
+
+                Column {
+                    anchors.margins:    10
+                    anchors.fill:       parent
+                    spacing:            10
+
+                    QGCLabel {
+                        color:  "yellow"
+                        text:   "<font color=\"yellow\">WARNING: Propellers must be removed from vehicle prior to performing UAVCAN ESC configuration.</font>"
+                    }
+
+                    QGCLabel {
+                        text: "You must use USB connection for this operation."
+                    }
+
+                    QGCButton {
+                        text:       "Configure"
+                        onClicked:  controller.busConfigureActuators()
+                    }
+                }
+            }
+
             /*
              * This is disabled for now
             Row {
diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.cc b/src/AutoPilotPlugins/PX4/PowerComponentController.cc
index 76218fc39..6892985e9 100644
--- a/src/AutoPilotPlugins/PX4/PowerComponentController.cc
+++ b/src/AutoPilotPlugins/PX4/PowerComponentController.cc
@@ -49,11 +49,23 @@ void PowerComponentController::calibrateEsc(void)
     _uas->startCalibration(UASInterface::StartCalibrationEsc);
 }
 
+void PowerComponentController::busConfigureActuators(void)
+{
+    _warningMessages.clear();
+    connect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
+    _uas->startBusConfig(UASInterface::StartBusConfigActuators);
+}
+
 void PowerComponentController::_stopCalibration(void)
 {
     disconnect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
 }
 
+void PowerComponentController::_stopBusConfig(void)
+{
+    _stopCalibration();
+}
+
 void PowerComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
 {
     Q_UNUSED(compId);
@@ -130,8 +142,28 @@ void PowerComponentController::_handleUASTextMessage(int uasId, int compId, int
         return;
     }
     
-    QString warningPrefix("calibration warning: ");
+    QString warningPrefix("config warning: ");
     if (text.startsWith(warningPrefix)) {
         _warningMessages << text.right(text.length() - warningPrefix.length());
     }
+
+    QString busFailedPrefix("bus conf fail:");
+    if (text.startsWith(busFailedPrefix)) {
+
+        _stopBusConfig();
+        emit calibrationFailed(text.right(text.length() - failedPrefix.length()));
+        return;
+    }
+
+    QString busCompletePrefix("bus conf done:");
+    if (text.startsWith(calCompletePrefix)) {
+        _stopBusConfig();
+        emit calibrationSuccess(_warningMessages);
+        return;
+    }
+
+    QString busWarningPrefix("bus conf warn: ");
+    if (text.startsWith(busWarningPrefix)) {
+        _warningMessages << text.right(text.length() - warningPrefix.length());
+    }
 }
diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.h b/src/AutoPilotPlugins/PX4/PowerComponentController.h
index 0b99874df..6e1b9388f 100644
--- a/src/AutoPilotPlugins/PX4/PowerComponentController.h
+++ b/src/AutoPilotPlugins/PX4/PowerComponentController.h
@@ -43,6 +43,7 @@ public:
     ~PowerComponentController();
     
     Q_INVOKABLE void calibrateEsc(void);
+    Q_INVOKABLE void busConfigureActuators(void);
     
 signals:
     void oldFirmware(void);
@@ -59,6 +60,7 @@ private slots:
     
 private:
     void _stopCalibration(void);
+    void _stopBusConfig(void);
     
     QStringList _warningMessages;
     static const int _neededFirmwareRev = 1;
-- 
2.22.0