Merge pull request #8236 from Williangalvani/motorcontroller

Support automatic Thruster direction detection in Ardusub

qgroundcontrol.pro

@@ -1027,6 +1027,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 \
@@ -1054,6 +1055,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

@@ -0,0 +1,30 @@
+/****************************************************************************
+ *
+ *   (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

@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ *   (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);