Merge pull request #7386 from DonLakeFlyer/SmartRTLEsc

ArduPilot: Smart RTL and ESC Calibration support

src/AutoPilotPlugins/APM/APMPowerComponent.qml

@@ -45,6 +45,8 @@ SetupPage {
             property bool _batt2ParamsAvailable:    controller.parameterExists(-1, "BATT2_CAPACITY")
             property bool _showBatt1Reboot:         _batt1MonitorEnabled && !_batt1ParamsAvailable
             property bool _showBatt2Reboot:         _batt2MonitorEnabled && !_batt2ParamsAvailable
+            property bool _escCalibrationAvailable: controller.parameterExists(-1, "ESC_CALIBRATION")
+            property Fact _escCalibration:          controller.getParameterFact(-1, "ESC_CALIBRATION", false /* reportMissing */)
 
             property string _restartRequired: qsTr("Requires vehicle reboot")
 
@@ -217,6 +219,61 @@ SetupPage {
                     }
                 }
             }
+
+            Column {
+                spacing:    _margins / 2
+                visible:    _escCalibrationAvailable
+
+                QGCLabel {
+                    text:       qsTr("ESC Calibration")
+                    font.family: ScreenTools.demiboldFontFamily
+                }
+
+                Rectangle {
+                    width:  escCalibrationHolder.x + escCalibrationHolder.width + _margins
+                    height: escCalibrationHolder.y + escCalibrationHolder.height + _margins
+                    color:  ggcPal.windowShade
+
+                    Column {
+                        id:         escCalibrationHolder
+                        x:          _margins
+                        y:          _margins
+                        spacing:    _margins
+
+                        Column {
+                            spacing: _margins
+
+                            QGCLabel {
+                                text:   qsTr("WARNING: Remove props prior to calibration!")
+                                color:  qgcPal.warningText
+                            }
+
+                            Row {
+                                spacing: _margins
+
+                                QGCButton {
+                                    text: qsTr("Calibrate")
+                                    enabled:    _escCalibration.rawValue === 0
+                                    onClicked:  _escCalibration.rawValue = 3
+                                }
+
+                                Column {
+                                    enabled: _escCalibration.rawValue === 3
+                                    QGCLabel { text:   _escCalibration.rawValue === 3 ? qsTr("Now perform these steps:") : qsTr("Click Calibrate to start, then:") }
+                                    QGCLabel { text:   qsTr("- Disconnect USB and battery so flight controller powers down") }
+                                    QGCLabel { text:   qsTr("- Connect the battery") }
+                                    QGCLabel { text:   qsTr("- The arming tone will be played (if the vehicle has a buzzer attached)") }
+                                    QGCLabel { text:   qsTr("- If using a flight controller with a safety button press it until it displays solid red") }
+                                    QGCLabel { text:   qsTr("- You will hear a musical tone then two beeps") }
+                                    QGCLabel { text:   qsTr("- A few seconds later you should hear a number of beeps (one for each battery cell you’re using)") }
+                                    QGCLabel { text:   qsTr("- And finally a single long beep indicating the end points have been set and the ESC is calibrated") }
+                                    QGCLabel { text:   qsTr("- Disconnect the battery and power up again normally") }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
         } // Flow
     } // Component - powerPageComponent
 

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -897,9 +897,9 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
     vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
 }
 
-void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
 {
-    _setFlightModeAndValidate(vehicle, rtlFlightMode());
+    _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode());
 }
 
 void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -86,9 +86,10 @@ public:
     bool                isGuidedMode                    (const Vehicle* vehicle) const override;
     QString             gotoFlightMode                  (void) const override { return QStringLiteral("Guided"); }
     QString             rtlFlightMode                   (void) const override { return QString("RTL"); }
+    QString             smartRTLFlightMode              (void) const override { return QString("Smart RTL"); }
     QString             missionFlightMode               (void) const override { return QString("Auto"); }
     void                pauseVehicle                    (Vehicle* vehicle) override;
-    void                guidedModeRTL                   (Vehicle* vehicle) override;
+    void                guidedModeRTL                   (Vehicle* vehicle, bool smartRTL) override;
     void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeChange) override;
     bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) override;
     void                adjustOutgoingMavlinkMessage    (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override;

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -43,7 +43,6 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
     enumToString.insert(GUIDED_NOGPS,   "Guided No GPS");
     enumToString.insert(SAFE_RTL,       "Smart RTL");
 
-
     setEnumToStringMapping(enumToString);
 }
 

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -65,8 +65,9 @@ public:
     QString pauseFlightMode                     (void) const override { return QString("Brake"); }
     QString landFlightMode                      (void) const override { return QString("Land"); }
     QString takeControlFlightMode               (void) const override { return QString("Loiter"); }
-    bool    vehicleYawsToNextWaypointInMission  (const Vehicle* vehicle) const final;
-    QString autoDisarmParameter                 (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
+    bool    vehicleYawsToNextWaypointInMission  (const Vehicle* vehicle) const override;
+    QString autoDisarmParameter                 (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
+    bool    supportsSmartRTL                    (void) const override { return true; }
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -51,6 +51,7 @@ public:
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const   FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     bool    supportsNegativeThrust                  (void) final;
+    bool    supportsSmartRTL                        (void) const override { return true; }
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -240,10 +240,11 @@ void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
     qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
 }
 
-void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
 {
     // Not supported by generic vehicle
     Q_UNUSED(vehicle);
+    Q_UNUSED(smartRTL);
     qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
 }
 

src/FirmwarePlugin/FirmwarePlugin.h

@@ -105,6 +105,11 @@ public:
     /// Returns the flight mode for RTL
     virtual QString rtlFlightMode(void) const { return QString(); }
 
+    /// Returns the flight mode for Smart RTL
+    virtual QString smartRTLFlightMode(void) const { return QString(); }
+
+    virtual bool supportsSmartRTL(void) const { return false; }
+
     /// Returns the flight mode for Land
     virtual QString landFlightMode(void) const { return QString(); }
 
@@ -125,7 +130,7 @@ public:
     virtual void pauseVehicle(Vehicle* vehicle);
 
     /// Command vehicle to return to launch
-    virtual void guidedModeRTL(Vehicle* vehicle);
+    virtual void guidedModeRTL(Vehicle* vehicle, bool smartRTL);
 
     /// Command vehicle to land at current location
     virtual void guidedModeLand(Vehicle* vehicle);

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -351,8 +351,9 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
                             NAN);
 }
 
-void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
 {
+    Q_UNUSED(smartRTL);
     _setFlightModeAndValidate(vehicle, _rtlFlightMode);
 }
 

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -44,7 +44,7 @@ public:
     QString             takeControlFlightMode           (void) const override { return _manualFlightMode; }
     QString             gotoFlightMode                  (void) const override { return _holdFlightMode; }
     void                pauseVehicle                    (Vehicle* vehicle) override;
-    void                guidedModeRTL                   (Vehicle* vehicle) override;
+    void                guidedModeRTL                   (Vehicle* vehicle, bool smartRTL) override;
     void                guidedModeLand                  (Vehicle* vehicle) override;
     void                guidedModeTakeoff               (Vehicle* vehicle, double takeoffAltRel) override;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;

src/FlightDisplay/GuidedActionConfirm.qml

@@ -35,6 +35,8 @@ Rectangle {
     property var    actionData
     property bool   hideTrigger:        false
     property var    mapIndicator
+    property alias  optionText:         optionCheckBox.text
+    property alias  optionChecked:      optionCheckBox.checked
 
     property real _margins:         ScreenTools.defaultFontPixelWidth
     property bool _emergencyAction: action === guidedController.actionEmergencyStop
@@ -101,6 +103,13 @@ Rectangle {
             wrapMode:               Text.WordWrap
         }
 
+        QGCCheckBox {
+            id:                         optionCheckBox
+            anchors.horizontalCenter:   parent.horizontalCenter
+            text:                       ""
+            visible:                    text !== ""
+        }
+
         // Action confirmation control
         SliderSwitch {
             id:             slider
@@ -115,7 +124,7 @@ Rectangle {
                     altitudeSlider.visible = false
                 }
                 hideTrigger = false
-                guidedController.executeAction(_root.action, _root.actionData, altitudeChange)
+                guidedController.executeAction(_root.action, _root.actionData, altitudeChange, _root.optionChecked)
                 if (mapIndicator) {
                     mapIndicator.actionConfirmed()
                     mapIndicator = undefined

src/FlightDisplay/GuidedActionsController.qml

@@ -203,7 +203,7 @@ Item {
 
     on_FlightModeChanged: {
         _vehiclePaused =        _activeVehicle ? _flightMode === _activeVehicle.pauseFlightMode : false
-        _vehicleInRTLMode =     _activeVehicle ? _flightMode === _activeVehicle.rtlFlightMode : false
+        _vehicleInRTLMode =     _activeVehicle ? _flightMode === _activeVehicle.rtlFlightMode || _flightMode === _activeVehicle.smartRTLFlightMode : false
         _vehicleInLandMode =    _activeVehicle ? _flightMode === _activeVehicle.landFlightMode : false
         _vehicleInMissionMode = _activeVehicle ? _flightMode === _activeVehicle.missionFlightMode : false // Must be last to get correct signalling for showStartMission popups
     }
@@ -216,6 +216,7 @@ Item {
         confirmDialog.actionData = actionData
         confirmDialog.hideTrigger = true
         confirmDialog.mapIndicator = mapIndicator
+        confirmDialog.optionText = ""
         _actionData = actionData
         switch (actionCode) {
         case actionArm:
@@ -279,6 +280,10 @@ Item {
         case actionRTL:
             confirmDialog.title = rtlTitle
             confirmDialog.message = rtlMessage
+            if (_activeVehicle.supportsSmartRTL) {
+                confirmDialog.optionText = qsTr("Smart RTL")
+                confirmDialog.optionChecked = false
+            }
             confirmDialog.hideTrigger = Qt.binding(function() { return !showRTL })
             break;
         case actionChangeAlt:
@@ -339,12 +344,12 @@ Item {
     }
 
     // Executes the specified action
-    function executeAction(actionCode, actionData, actionAltitudeChange) {
+    function executeAction(actionCode, actionData, actionAltitudeChange, optionChecked) {
         var i;
         var rgVehicle;
         switch (actionCode) {
         case actionRTL:
-            _activeVehicle.guidedModeRTL()
+            _activeVehicle.guidedModeRTL(optionChecked)
             break
         case actionLand:
             _activeVehicle.guidedModeLand()

src/Vehicle/Vehicle.cc

@@ -2975,13 +2975,13 @@ QString Vehicle::gotoFlightMode() const
     return _firmwarePlugin->gotoFlightMode();
 }
 
-void Vehicle::guidedModeRTL(void)
+void Vehicle::guidedModeRTL(bool smartRTL)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
         return;
     }
-    _firmwarePlugin->guidedModeRTL(this);
+    _firmwarePlugin->guidedModeRTL(this, smartRTL);
 }
 
 void Vehicle::guidedModeLand(void)
@@ -3602,6 +3602,16 @@ QString Vehicle::rtlFlightMode(void) const
     return _firmwarePlugin->rtlFlightMode();
 }
 
+QString Vehicle::smartRTLFlightMode(void) const
+{
+    return _firmwarePlugin->smartRTLFlightMode();
+}
+
+bool Vehicle::supportsSmartRTL(void) const
+{
+    return _firmwarePlugin->supportsSmartRTL();
+}
+
 QString Vehicle::landFlightMode(void) const
 {
     return _firmwarePlugin->landFlightMode();

src/Vehicle/Vehicle.h

@@ -600,6 +600,8 @@ public:
     Q_PROPERTY(QString              missionFlightMode       READ missionFlightMode                                      CONSTANT)
     Q_PROPERTY(QString              pauseFlightMode         READ pauseFlightMode                                        CONSTANT)
     Q_PROPERTY(QString              rtlFlightMode           READ rtlFlightMode                                          CONSTANT)
+    Q_PROPERTY(QString              smartRTLFlightMode      READ smartRTLFlightMode                                     CONSTANT)
+    Q_PROPERTY(bool                 supportsSmartRTL        READ supportsSmartRTL                                       CONSTANT)
     Q_PROPERTY(QString              landFlightMode          READ landFlightMode                                         CONSTANT)
     Q_PROPERTY(QString              takeControlFlightMode   READ takeControlFlightMode                                  CONSTANT)
     Q_PROPERTY(QString              firmwareTypeString      READ firmwareTypeString                                     NOTIFY firmwareTypeChanged)
@@ -698,7 +700,7 @@ public:
     Q_INVOKABLE void disconnectInactiveVehicle(void);
 
     /// Command vehicle to return to launch
-    Q_INVOKABLE void guidedModeRTL(void);
+    Q_INVOKABLE void guidedModeRTL(bool smartRTL);
 
     /// Command vehicle to land at current location
     Q_INVOKABLE void guidedModeLand(void);
@@ -919,6 +921,8 @@ public:
     QString         missionFlightMode       () const;
     QString         pauseFlightMode         () const;
     QString         rtlFlightMode           () const;
+    QString         smartRTLFlightMode      () const;
+    bool            supportsSmartRTL        () const;
     QString         landFlightMode          () const;
     QString         takeControlFlightMode   () const;
     double          defaultCruiseSpeed      () const { return _defaultCruiseSpeed; }