Add periodic RC override sending.

Fix PWM input fields update

Author: zdanek

src/AnalyzeView/MAVLinkInspectorController.h

@@ -95,6 +95,7 @@ class QGCMAVLinkMessage : public QObject {
     Q_PROPERTY(QString              name            READ name           CONSTANT)
     Q_PROPERTY(qreal                messageHz       READ messageHz      NOTIFY freqChanged)
     Q_PROPERTY(quint64              count           READ count          NOTIFY countChanged)
+    Q_PROPERTY(int                  mavlinkVer      READ mavlinkVer     CONSTANT)
     Q_PROPERTY(QmlObjectListModel*  fields          READ fields         CONSTANT)
     Q_PROPERTY(bool                 fieldSelected   READ fieldSelected  NOTIFY fieldSelectedChanged)
     Q_PROPERTY(bool                 selected        READ selected       NOTIFY selectedChanged)
@@ -107,6 +108,7 @@ class QGCMAVLinkMessage : public QObject {
     QString             name            () { return _name;  }
     qreal               messageHz       () const{ return _messageHz; }
     quint64             count           () const{ return _count; }
+    int                 mavlinkVer      () const{ return _message.magic == 0xFE ? 1 : 2; }
     quint64             lastCount       () const{ return _lastCount; }
     QmlObjectListModel* fields          () { return &_fields; }
     bool                fieldSelected   () const{ return _fieldSelected; }

src/AnalyzeView/MAVLinkInspectorPage.qml

@@ -158,6 +158,12 @@ AnalyzePage {
                         QGCLabel {
                             text:       curMessage ? curMessage.count : ""
                         }
+                        QGCLabel {
+                            text:       qsTr("Mavlink ver:")
+                        }
+                        QGCLabel {
+                            text:       curMessage ? curMessage.mavlinkVer : ""
+                        }
                     }
                     Item { height: ScreenTools.defaultFontPixelHeight; width: 1 }
                     //---------------------------------------------------------

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -260,6 +260,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* /*vehicle*/, mavlink_
     // and lower the severity on them so that they don't pop in the users face.
 
     QString messageText = _getMessageText(message);
+    qDebug(APMFirmwarePluginLog) << "StatusText: " << messageText;
     if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
         _adjustCalibrationMessageSeverity(message);
         return true;
@@ -1047,3 +1048,8 @@ QMutex& APMFirmwarePlugin::_reencodeMavlinkChannelMutex()
     static QMutex _mutex{};
     return _mutex;
 }
+
+bool APMFirmwarePlugin::supportsRcChannelOverride(void)
+{
+    return true;
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -76,7 +76,8 @@ class APMFirmwarePlugin : public FirmwarePlugin
     QObject*            _loadParameterMetaData          (const QString& metaDataFile) override;
     QString             brandImageIndoor                (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
     QString             brandImageOutdoor               (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
-    QString             getHobbsMeter                   (Vehicle* vehicle) override; 
+    QString             getHobbsMeter                   (Vehicle* vehicle) override;
+    bool supportsRcChannelOverride(void) override;
 
 protected:
     /// All access to singleton is through stack specific implementation

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -154,6 +154,11 @@ bool FirmwarePlugin::supportsJSButton(void)
     return false;
 }
 
+bool FirmwarePlugin::supportsRcChannelOverride(void)
+{
+    return false;
+}
+
 bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
     Q_UNUSED(vehicle);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -205,6 +205,10 @@ class FirmwarePlugin : public QObject
     /// to be assigned via parameters in firmware. Default is false.
     virtual bool supportsJSButton(void);
 
+    /// Returns true if the firmware supports the mavlink RC_CHANNELS_OVERRIDE message
+    /// Default is false.
+    virtual bool supportsRcChannelOverride(void);
+
     /// Returns true if the firmware supports calibrating motor interference offsets for the compass
     /// (CompassMot). Default is true.
     virtual bool supportsMotorInterference(void);

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -728,3 +728,8 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
     qCDebug(VehicleLog) << "Hobbs Meter string:" << timeStr;
     return timeStr;
 }
+
+bool PX4FirmwarePlugin::supportsRcChannelOverride(void)
+{
+    return false;
+}

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -72,6 +72,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
     uint32_t            highLatencyCustomModeTo32Bits   (uint16_t hlCustomMode) override;
     bool                supportsNegativeThrust          (Vehicle* vehicle) override;
     QString             getHobbsMeter                   (Vehicle* vehicle) override;
+    bool                supportsRcChannelOverride       (void) override;
 
 protected:
     typedef struct {

src/Joystick/Joystick.cc

@@ -10,7 +10,6 @@
 
 #include "Joystick.h"
 #include "QGC.h"
-#include "AutoPilotPlugin.h"
 #include "UAS.h"
 #include "QGCApplication.h"
 #include "VideoManager.h"
@@ -88,6 +87,7 @@ const float Joystick::_minAxisFrequencyHz       = 0.25f;
 const float Joystick::_maxAxisFrequencyHz       = 200.0f;
 const float Joystick::_minButtonFrequencyHz     = 0.25f;
 const float Joystick::_maxButtonFrequencyHz     = 50.0f;
+const float Joystick::_rcOverrideFrequencyHz    = 0.5f;
 
 AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action)
     : QObject(parent)
@@ -109,28 +109,28 @@ AssignedButtonAction::AssignedButtonAction(
 {
 }
 
-void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown)
+int16_t AssignedButtonAction::calculatePwm(Vehicle *vehicle, bool buttonDown)
 {
     if (!_isPwmOverrideAction) {
         qCWarning(JoystickLog) << "send called on non-pwm action";
-        return;
+        return -1;
     }
 
     uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue;
     if (_pwmLatchMode) {
         qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_pwmLatchButtonDown ? "down" : "up");
 
         if (buttonDown) {
+            // altering latch state
             _pwmLatchButtonDown = !_pwmLatchButtonDown;
-            pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue;
-        } else {
-            qCDebug(JoystickLog) << "since button is up - exiting";
-            return;
         }
+        pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue;
     }
 
-    qCDebug(JoystickLog) << " Sending PWM Value " << pwmValue;
-    vehicle->rcChannelOverride(_pwmRcChannel, pwmValue);
+    qCDebug(JoystickLog) << " Calculated PWM Value " << pwmValue << " for action " << _action;
+    //vehicle->rcChannelOverride(_pwmRcChannel, pwmValue);
+
+    return pwmValue;
 }
 
 uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action)
@@ -162,11 +162,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     , _totalButtonCount(_buttonCount+_hatButtonCount)
     , _multiVehicleManager(multiVehicleManager)
 {
-<<<<<<< HEAD
     qRegisterMetaType<GRIPPER_ACTIONS>();
 
-=======
->>>>>>> Refactor for clearness
     _rgAxisValues   = new int[static_cast<size_t>(_axisCount)];
     _rgCalibration  = new Calibration_t[static_cast<size_t>(_axisCount)];
     _rgButtonValues = new uint8_t[static_cast<size_t>(_totalButtonCount)];
@@ -177,6 +174,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
         _rgButtonValues[i] = BUTTON_UP;
         _buttonActionArray.append(nullptr);
     }
+
     _buildActionList(_multiVehicleManager->activeVehicle());
     _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
     _loadSettings();
@@ -277,6 +275,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
         int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
         setTXMode(mode);
     }
+    _clearRcOverrideButtonActions();
 }
 
 void Joystick::_loadSettings()
@@ -384,7 +383,7 @@ void Joystick::_loadSettings()
             _buttonActionArray[button]->buttonTime().start();
 
             bool savedRepeatState = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool();
-            setButtonRepeatIfAvailable(button, savedRepeatState);
+            _setButtonRepeatIfAvailable(button, savedRepeatState);
 
             qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action()
                                  << _buttonActionArray[button]->repeat();
@@ -411,7 +410,6 @@ void Joystick::_saveButtonSettings()
                 settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button),  _buttonActionArray[button]->lowPwm());
                 settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm());
                 settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode());
-
             }
         }
     }
@@ -567,6 +565,7 @@ void Joystick::run()
     _open();
     //-- Reset timers
     _axisTime.start();
+    _rcOverrideTimer.start();
     for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
         if(_buttonActionArray[buttonIndex]) {
             _buttonActionArray[buttonIndex]->buttonTime().start();
@@ -576,6 +575,7 @@ void Joystick::run()
         _update();
         _handleButtons();
         _handleAxis();
+        _handleRcOverride();
         QGC::SLEEP::msleep(qMin(static_cast<int>(1000.0f / _maxAxisFrequencyHz), static_cast<int>(1000.0f / _maxButtonFrequencyHz)) / 2);
     }
     _close();
@@ -773,6 +773,26 @@ void Joystick::_handleAxis()
     }
 }
 
+void Joystick::_handleRcOverride()
+{
+    if (!_activeVehicle || !_activeVehicle->supportsRcChannelOverride()) {
+        return;
+    }
+    const int rcOverrideInterval = static_cast<int>(1000.0f / _rcOverrideFrequencyHz);
+
+    if (_rcOverrideActive || _rcOverrideTimer.elapsed() > rcOverrideInterval) {
+        _rcOverrideActive = false;
+        _rcOverrideTimer.start();
+        uint16_t overrideData[18] = {UINT16_MAX};
+        overrideData[16] = _mapRcOverrideToRelease(17, 0);
+        overrideData[17] = _mapRcOverrideToRelease(18, 0);
+        for (int i = 0; i < MAX_RC_CHANNELS; i++) {
+            overrideData[i] = _rcOverride[i] == -1 ? _mapRcOverrideToRelease(i + 1, 0) : _rcOverride[i];
+        }
+        _activeVehicle->rcChannelsOverride(overrideData);
+    }
+}
+
 void Joystick::startPolling(Vehicle* vehicle)
 {
     if (vehicle) {
@@ -905,7 +925,7 @@ void Joystick::setButtonAction(int button, const QString& action)
     if (action.isEmpty() || action == _buttonActionNone) {
         _pwmSettingsVisibilities[button] = false;
         if (_buttonActionArray[button]) {
-            removeButtonSettings(button);
+            _removeButtonSettings(button);
             _buttonActionArray[button]->deleteLater();
             _buttonActionArray[button] = nullptr;
         }
@@ -915,7 +935,7 @@ void Joystick::setButtonAction(int button, const QString& action)
         qCDebug(JoystickLog) << "setButtonAction: isPwmAction " << isPwmAction;
 
         if (!_buttonActionArray[button]) {
-            _buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1000, 2000, false)
+            _buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1500, 1500, false)
                                                      : new AssignedButtonAction(this, action);
         } else {
             if (isPwmAction) {
@@ -933,14 +953,14 @@ void Joystick::setButtonAction(int button, const QString& action)
         }
 
         //-- Make sure repeat is off if this action doesn't support repeats
-        setButtonRepeatIfAvailable(button, false);
-        saveButtonSettings(button);
+        _setButtonRepeatIfAvailable(button, false);
+        _saveButtonSettings(button);
     }
     emit pwmVisibilitiesChanged();
     emit buttonActionsChanged();
 }
 
-void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
+void Joystick::_setButtonRepeatIfAvailable(int button, bool repeat)
 {
     int idx = _findAssignableButtonAction(_buttonActionArray[button]->action());
     if (idx >= 0) {
@@ -953,7 +973,7 @@ void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
     _buttonActionArray[button]->repeat(false);
 }
 
-void Joystick::saveButtonSettings(int button)
+void Joystick::_saveButtonSettings(int button)
 {
     QSettings settings;
     settings.beginGroup(_settingsGroup);
@@ -968,7 +988,7 @@ void Joystick::saveButtonSettings(int button)
     }
 }
 
-void Joystick::removeButtonSettings(int button)
+void Joystick::_removeButtonSettings(int button)
 {
     QSettings settings;
     settings.beginGroup(_settingsGroup);
@@ -1347,10 +1367,10 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
 
-    // TODO(bzd) take channel nos from config, especially max
-    for (int ch = 8; ch <= 16;ch++) {
-         _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
+    for (int ch = 8; ch <= MAX_RC_CHANNELS; ch++) {
+        _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
     }
+    _clearRcOverrideButtonActions();
 
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
     for (auto& item : _customMavCommands) {
@@ -1367,8 +1387,27 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
 bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown)
 {
     if (_buttonActionArray[buttonIndex]) {
-        _buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown);
+        uint8_t channel = _buttonActionArray[buttonIndex]->rcChannel();
+        auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(_activeVehicle, buttonDown);
+        _rcOverride[channel-1] = pwm;
+        _rcOverrideActive = true;
         return true;
     }
     return  false;
 }
+
+void Joystick::_clearRcOverrideButtonActions() {
+    qCDebug(JoystickLog) << "Clearing RC override button actions";
+    for (int i = 0; i < MAX_RC_CHANNELS; i++) {
+        _rcOverride[i] = -1;
+    }
+}
+
+uint16_t Joystick::_mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value) {
+    // UINT16_MAX - ignore
+    // 0 Release
+    if (value == 0) {
+        return rcChannel < 9 ? 0 : UINT16_MAX - 1;
+    }
+    return value;
+}