Support automatic Thruster direction detection in Ardusub

This adds support for the new ArduSub 3.0 feature of automatically detecting
the thrusters directions and reverting the corresponding motor output if
necessary

qgroundcontrol.pro

@@ -1105,6 +1105,7 @@ APMFirmwarePlugin {
         src/AutoPilotPlugins/APM/APMSafetyComponent.h \
         src/AutoPilotPlugins/APM/APMSensorsComponent.h \
         src/AutoPilotPlugins/APM/APMSensorsComponentController.h \
+        src/AutoPilotPlugins/APM/APMSubMotorComponentController.h \
         src/AutoPilotPlugins/APM/APMTuningComponent.h \
         src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
         src/FirmwarePlugin/APM/APMParameterMetaData.h \
@@ -1132,6 +1133,7 @@ APMFirmwarePlugin {
         src/AutoPilotPlugins/APM/APMSafetyComponent.cc \
         src/AutoPilotPlugins/APM/APMSensorsComponent.cc \
         src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \
+        src/AutoPilotPlugins/APM/APMSubMotorComponentController.cc \
         src/AutoPilotPlugins/APM/APMTuningComponent.cc \
         src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
         src/FirmwarePlugin/APM/APMParameterMetaData.cc \

src/AutoPilotPlugins/APM/APMMotorComponent.cc

@@ -8,6 +8,7 @@
  ****************************************************************************/
 
 #include "APMMotorComponent.h"
+#include "APMSubMotorComponentController.h"
 
 APMMotorComponent::APMMotorComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) :
     MotorComponent(vehicle, autopilot, parent),

src/AutoPilotPlugins/APM/APMSubMotorComponent.qml

@@ -15,6 +15,7 @@ import QGroundControl               1.0
 import QGroundControl.Controls      1.0
 import QGroundControl.FactSystem    1.0
 import QGroundControl.ScreenTools   1.0
+import QGroundControl.Controllers   1.0
 
 SetupPage {
     id:             motorPage
@@ -27,8 +28,9 @@ SetupPage {
 
     property int neutralValue: 50;
     property int _lastIndex: 0;
+    property bool canDoManualTest: controller.vehicle.flightMode !== 'Motor Detection' && controller.vehicle.armed && motorPage.visible
 
-    FactPanelController {
+    APMSubMotorComponentController {
         id:             controller
     }
 
@@ -47,7 +49,7 @@ SetupPage {
 
             Row {
                 id:         motorSliders
-                enabled:    controller.vehicle.armed
+                enabled:    canDoManualTest
                 spacing:    ScreenTools.defaultFontPixelWidth * 4
 
                 Column {
@@ -199,22 +201,84 @@ SetupPage {
                 QGCLabel {
                     anchors.verticalCenter: safetySwitch.verticalCenter
                     color:  qgcPal.warningText
-                    text:   qsTr("Slide this switch to arm the vehicle and enable the motor test (CAUTION!)")
+                    text:   coolDownTimer.running
+                                ? qsTr("A 10 second coooldown is required before testing again, please stand by...")
+                                : qsTr("Slide this switch to arm the vehicle and enable the motor test (CAUTION!)")
                 }
             } // Row
+
+            QGCLabel {
+                visible:             controller.vehicle.versionCompare(4, 0, 0) >= 0
+                width:               parent.width
+                anchors.left:        parent.left
+                anchors.right:       parent.right
+                font.pointSize:      ScreenTools.largeFontPointSize
+                text:                qsTr("Automatic Motor Direction Detection")
+            }
+
+            QGCLabel {
+                visible:        controller.vehicle.versionCompare(4, 0, 0) >= 0
+                anchors.left:   parent.left
+                anchors.right:  parent.right
+                wrapMode:       Text.WordWrap
+                text:           qsTr("This will attempt to automatically detect the direction (normal/reversed) of your thrusters.\n"
+                                   + "Please place your vehicle in water, click the button, and wait. Note that the thrusters still need "
+                                   + "to be connected to the correct outputs (thrusters 2 and 3 can't be swapped, for example).")
+            }
+
             Row {
-                QGCLabel {
-                    id: cooldownLabel
-                    visible: coolDownTimer.running
-                    color:  qgcPal.warningText
-                    text:   qsTr("A 10 second coooldown is required before testing again, please stand by...")
+                visible:    controller.vehicle.versionCompare(4, 0, 0) >= 0
+                spacing:    ScreenTools.defaultFontPixelWidth
+
+                Column {
+                    spacing:    ScreenTools.defaultFontPixelWidth * 2
+
+                    QGCButton {
+                        id: startAutoDetection
+                        text: "Auto-Detect Directions"
+                        enabled: controller.vehicle.flightMode !== 'Motor Detection'
+
+                        onClicked: function() {
+                            controller.vehicle.flightMode = "Motor Detection"
+                            controller.vehicle.armed = true
+                        }
+                    }
+                }
+                Column {
+                    spacing:    ScreenTools.defaultFontPixelWidth * 2
+
+                    Flickable {
+                        id: flickable
+                        width: 500
+                        height: Math.min(contentHeight, 200)
+                        contentWidth: width
+                        contentHeight: textArea.implicitHeight
+                        clip: true
+
+                        TextArea {
+                            id: textArea
+                            anchors.fill: parent
+                            color:  qgcPal.text
+                            text: controller.motorDetectionMessages
+                            wrapMode: Text.WordWrap
+                            background: Rectangle {
+                                color: qgcPal.window
+                            }
+                            onTextChanged: function() {
+                                flickable.flick(0, -300)
+                            }
+                        }
+                        ScrollBar.vertical: ScrollBar {}
+                    }
                 }
             }
+
             // Repeats the command signal and updates the checkbox every 50 ms
             Timer {
                 id: timer
                 interval:       50
                 repeat:         true
+                running:        canDoManualTest
 
                 onTriggered: {
                     if (controller.vehicle.armed) {

src/AutoPilotPlugins/APM/APMSubMotorComponentController.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#include "APMSubMotorComponentController.h"
+#include "ParameterManager.h"
+
+
+APMSubMotorComponentController::APMSubMotorComponentController(void)
+{   
+    connect(_vehicle, &Vehicle::textMessageReceived, this, &APMSubMotorComponentController::handleNewMessages);
+}
+
+void APMSubMotorComponentController::handleNewMessages(int uasid, int componentid, int severity, QString text)
+{
+    Q_UNUSED(uasid);
+    Q_UNUSED(componentid);
+    Q_UNUSED(severity);
+    if (_vehicle->flightMode() == "Motor Detection"
+        && (text.toLower().contains("thruster") || text.toLower().contains("motor"))) {
+        _motorDetectionMessages += text + QStringLiteral("\n");
+        emit motorDetectionMessagesChanged();
+    }
+}

src/AutoPilotPlugins/APM/APMSubMotorComponentController.h

+/****************************************************************************
+ *
+ *   (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#ifndef APMSubMotorComponentController_H
+#define APMSubMotorComponentController_H
+
+#include <QObject>
+#include "FactPanelController.h"
+#include "Vehicle.h"
+
+/// MVC Controller for APMSubMotorComponent.qml.
+class APMSubMotorComponentController : public FactPanelController
+{
+    Q_OBJECT
+
+public:
+    APMSubMotorComponentController(void);
+    Q_PROPERTY(QString motorDetectionMessages READ motorDetectionMessages NOTIFY motorDetectionMessagesChanged);
+    QString motorDetectionMessages() const {return _motorDetectionMessages;};
+
+signals:
+    void motorDetectionMessagesChanged();
+
+private slots:
+    void handleNewMessages(int uasid, int componentid, int severity, QString text);
+
+private:
+    QString _motorDetectionMessages;
+};
+
+#endif

src/AutoPilotPlugins/CMakeLists.txt

@@ -22,6 +22,7 @@ add_library(AutoPilotPlugins
 	APM/APMSensorsComponent.cc
 	APM/APMSensorsComponentController.cc
 	APM/APMSubFrameComponent.cc
+	APM/APMSubMotorComponentController.cc
 	APM/APMTuningComponent.cc
 
 	Common/ESP8266Component.cc

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -15,6 +15,7 @@
 #include "APMAirframeComponentController.h"
 #include "APMSensorsComponentController.h"
 #include "APMFollowComponentController.h"
+#include "APMSubMotorComponentController.h"
 #include "MissionManager.h"
 #include "ParameterManager.h"
 #include "QGCFileDownload.h"
@@ -160,6 +161,7 @@ APMFirmwarePlugin::APMFirmwarePlugin(void)
     qmlRegisterType<APMAirframeComponentController>     ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController");
     qmlRegisterType<APMSensorsComponentController>      ("QGroundControl.Controllers", 1, 0, "APMSensorsComponentController");
     qmlRegisterType<APMFollowComponentController>       ("QGroundControl.Controllers", 1, 0, "APMFollowComponentController");
+    qmlRegisterType<APMSubMotorComponentController>     ("QGroundControl.Controllers", 1, 0, "APMSubMotorComponentController");
 }
 
 AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc

@@ -42,6 +42,7 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) :
         {CIRCLE, "Circle"},
         {SURFACE, "Surface"},
         {POSHOLD, "Position Hold"},
+        {MOTORDETECTION, "Motor Detection"},
     });
 }
 
@@ -58,6 +59,7 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
         APMSubMode(APMSubMode::CIRCLE ,true),
         APMSubMode(APMSubMode::SURFACE ,false),
         APMSubMode(APMSubMode::POSHOLD ,true),
+        APMSubMode(APMSubMode::MOTORDETECTION, false),
     });
 
     if (!_remapParamNameIntialized) {

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h

@@ -94,7 +94,8 @@ public:
         POSHOLD           = 16,  // Hold position
         RESERVED_17       = 17,
         RESERVED_18       = 18,
-        MANUAL            = 19
+        MANUAL            = 19,
+        MOTORDETECTION    = 20,
     };
 
     APMSubMode(uint32_t mode, bool settable);