Skip to content

Commit 76fa87e

Browse files
zdanekzdanek
committed
Add periodic RC override sending.
Fix PWM input fields update
1 parent 09ba12d commit 76fa87e

14 files changed

+155
-47
lines changed

‎src/AnalyzeView/MAVLinkInspectorController.h

+2
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,7 @@ class QGCMAVLinkMessage : public QObject {
9595
Q_PROPERTY(QString name READ name CONSTANT)
9696
Q_PROPERTY(qreal messageHz READ messageHz NOTIFY freqChanged)
9797
Q_PROPERTY(quint64 count READ count NOTIFY countChanged)
98+
Q_PROPERTY(int mavlinkVer READ mavlinkVer CONSTANT)
9899
Q_PROPERTY(QmlObjectListModel* fields READ fields CONSTANT)
99100
Q_PROPERTY(bool fieldSelected READ fieldSelected NOTIFY fieldSelectedChanged)
100101
Q_PROPERTY(bool selected READ selected NOTIFY selectedChanged)
@@ -107,6 +108,7 @@ class QGCMAVLinkMessage : public QObject {
107108
QString name () { return _name; }
108109
qreal messageHz () const{ return _messageHz; }
109110
quint64 count () const{ return _count; }
111+
int mavlinkVer () const{ return _message.magic == 0xFE ? 1 : 2; }
110112
quint64 lastCount () const{ return _lastCount; }
111113
QmlObjectListModel* fields () { return &_fields; }
112114
bool fieldSelected () const{ return _fieldSelected; }

‎src/AnalyzeView/MAVLinkInspectorPage.qml

+6
Original file line numberDiff line numberDiff line change
@@ -158,6 +158,12 @@ AnalyzePage {
158158
QGCLabel {
159159
text: curMessage ? curMessage.count : ""
160160
}
161+
QGCLabel {
162+
text: qsTr("Mavlink ver:")
163+
}
164+
QGCLabel {
165+
text: curMessage ? curMessage.mavlinkVer : ""
166+
}
161167
}
162168
Item { height: ScreenTools.defaultFontPixelHeight; width: 1 }
163169
//---------------------------------------------------------

‎src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

+6
Original file line numberDiff line numberDiff line change
@@ -260,6 +260,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* /*vehicle*/, mavlink_
260260
// and lower the severity on them so that they don't pop in the users face.
261261

262262
QString messageText = _getMessageText(message);
263+
qDebug(APMFirmwarePluginLog) << "StatusText: " << messageText;
263264
if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
264265
_adjustCalibrationMessageSeverity(message);
265266
return true;
@@ -1047,3 +1048,8 @@ QMutex& APMFirmwarePlugin::_reencodeMavlinkChannelMutex()
10471048
static QMutex _mutex{};
10481049
return _mutex;
10491050
}
1051+
1052+
bool APMFirmwarePlugin::supportsRcChannelOverride(void)
1053+
{
1054+
return true;
1055+
}

‎src/FirmwarePlugin/APM/APMFirmwarePlugin.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,8 @@ class APMFirmwarePlugin : public FirmwarePlugin
7676
QObject* _loadParameterMetaData (const QString& metaDataFile) override;
7777
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
7878
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
79-
QString getHobbsMeter (Vehicle* vehicle) override;
79+
QString getHobbsMeter (Vehicle* vehicle) override;
80+
bool supportsRcChannelOverride(void) override;
8081

8182
protected:
8283
/// All access to singleton is through stack specific implementation

‎src/FirmwarePlugin/FirmwarePlugin.cc

+5
Original file line numberDiff line numberDiff line change
@@ -154,6 +154,11 @@ bool FirmwarePlugin::supportsJSButton(void)
154154
return false;
155155
}
156156

157+
bool FirmwarePlugin::supportsRcChannelOverride(void)
158+
{
159+
return false;
160+
}
161+
157162
bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
158163
{
159164
Q_UNUSED(vehicle);

‎src/FirmwarePlugin/FirmwarePlugin.h

+4
Original file line numberDiff line numberDiff line change
@@ -205,6 +205,10 @@ class FirmwarePlugin : public QObject
205205
/// to be assigned via parameters in firmware. Default is false.
206206
virtual bool supportsJSButton(void);
207207

208+
/// Returns true if the firmware supports the mavlink RC_CHANNELS_OVERRIDE message
209+
/// Default is false.
210+
virtual bool supportsRcChannelOverride(void);
211+
208212
/// Returns true if the firmware supports calibrating motor interference offsets for the compass
209213
/// (CompassMot). Default is true.
210214
virtual bool supportsMotorInterference(void);

‎src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

+5
Original file line numberDiff line numberDiff line change
@@ -728,3 +728,8 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
728728
qCDebug(VehicleLog) << "Hobbs Meter string:" << timeStr;
729729
return timeStr;
730730
}
731+
732+
bool PX4FirmwarePlugin::supportsRcChannelOverride(void)
733+
{
734+
return false;
735+
}

‎src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

+1
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin
7272
uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override;
7373
bool supportsNegativeThrust (Vehicle* vehicle) override;
7474
QString getHobbsMeter (Vehicle* vehicle) override;
75+
bool supportsRcChannelOverride (void) override;
7576

7677
protected:
7778
typedef struct {

‎src/Joystick/Joystick.cc

+64-22
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,6 @@
1010

1111
#include "Joystick.h"
1212
#include "QGC.h"
13-
#include "AutoPilotPlugin.h"
1413
#include "UAS.h"
1514
#include "QGCApplication.h"
1615
#include "VideoManager.h"
@@ -88,6 +87,7 @@ const float Joystick::_minAxisFrequencyHz = 0.25f;
8887
const float Joystick::_maxAxisFrequencyHz = 200.0f;
8988
const float Joystick::_minButtonFrequencyHz = 0.25f;
9089
const float Joystick::_maxButtonFrequencyHz = 50.0f;
90+
const float Joystick::_rcOverrideFrequencyHz = 0.5f;
9191

9292
AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action)
9393
: QObject(parent)
@@ -109,28 +109,28 @@ AssignedButtonAction::AssignedButtonAction(
109109
{
110110
}
111111

112-
void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown)
112+
int16_t AssignedButtonAction::calculatePwm(Vehicle *vehicle, bool buttonDown)
113113
{
114114
if (!_isPwmOverrideAction) {
115115
qCWarning(JoystickLog) << "send called on non-pwm action";
116-
return;
116+
return -1;
117117
}
118118

119119
uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue;
120120
if (_pwmLatchMode) {
121121
qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_pwmLatchButtonDown ? "down" : "up");
122122

123123
if (buttonDown) {
124+
// altering latch state
124125
_pwmLatchButtonDown = !_pwmLatchButtonDown;
125-
pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue;
126-
} else {
127-
qCDebug(JoystickLog) << "since button is up - exiting";
128-
return;
129126
}
127+
pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue;
130128
}
131129

132-
qCDebug(JoystickLog) << " Sending PWM Value " << pwmValue;
133-
vehicle->rcChannelOverride(_pwmRcChannel, pwmValue);
130+
qCDebug(JoystickLog) << " Calculated PWM Value " << pwmValue << " for action " << _action;
131+
//vehicle->rcChannelOverride(_pwmRcChannel, pwmValue);
132+
133+
return pwmValue;
134134
}
135135

136136
uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action)
@@ -177,6 +177,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
177177
_rgButtonValues[i] = BUTTON_UP;
178178
_buttonActionArray.append(nullptr);
179179
}
180+
180181
_buildActionList(_multiVehicleManager->activeVehicle());
181182
_updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
182183
_loadSettings();
@@ -277,6 +278,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
277278
int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
278279
setTXMode(mode);
279280
}
281+
_clearRcOverrideButtonActions();
280282
}
281283

282284
void Joystick::_loadSettings()
@@ -384,7 +386,7 @@ void Joystick::_loadSettings()
384386
_buttonActionArray[button]->buttonTime().start();
385387

386388
bool savedRepeatState = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool();
387-
setButtonRepeatIfAvailable(button, savedRepeatState);
389+
_setButtonRepeatIfAvailable(button, savedRepeatState);
388390

389391
qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action()
390392
<< _buttonActionArray[button]->repeat();
@@ -411,7 +413,6 @@ void Joystick::_saveButtonSettings()
411413
settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm());
412414
settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm());
413415
settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode());
414-
415416
}
416417
}
417418
}
@@ -567,6 +568,7 @@ void Joystick::run()
567568
_open();
568569
//-- Reset timers
569570
_axisTime.start();
571+
_rcOverrideTimer.start();
570572
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
571573
if(_buttonActionArray[buttonIndex]) {
572574
_buttonActionArray[buttonIndex]->buttonTime().start();
@@ -576,6 +578,7 @@ void Joystick::run()
576578
_update();
577579
_handleButtons();
578580
_handleAxis();
581+
_handleRcOverride();
579582
QGC::SLEEP::msleep(qMin(static_cast<int>(1000.0f / _maxAxisFrequencyHz), static_cast<int>(1000.0f / _maxButtonFrequencyHz)) / 2);
580583
}
581584
_close();
@@ -773,6 +776,26 @@ void Joystick::_handleAxis()
773776
}
774777
}
775778

779+
void Joystick::_handleRcOverride()
780+
{
781+
if (!_activeVehicle || !_activeVehicle->supportsRcChannelOverride()) {
782+
return;
783+
}
784+
const int rcOverrideInterval = static_cast<int>(1000.0f / _rcOverrideFrequencyHz);
785+
786+
if (_rcOverrideActive || _rcOverrideTimer.elapsed() > rcOverrideInterval) {
787+
_rcOverrideActive = false;
788+
_rcOverrideTimer.start();
789+
uint16_t overrideData[18] = {UINT16_MAX};
790+
overrideData[16] = _mapRcOverrideToRelease(17, 0);
791+
overrideData[17] = _mapRcOverrideToRelease(18, 0);
792+
for (int i = 0; i < MAX_RC_CHANNELS; i++) {
793+
overrideData[i] = _rcOverride[i] == -1 ? _mapRcOverrideToRelease(i + 1, 0) : _rcOverride[i];
794+
}
795+
_activeVehicle->rcChannelsOverride(overrideData);
796+
}
797+
}
798+
776799
void Joystick::startPolling(Vehicle* vehicle)
777800
{
778801
if (vehicle) {
@@ -905,7 +928,7 @@ void Joystick::setButtonAction(int button, const QString& action)
905928
if (action.isEmpty() || action == _buttonActionNone) {
906929
_pwmSettingsVisibilities[button] = false;
907930
if (_buttonActionArray[button]) {
908-
removeButtonSettings(button);
931+
_removeButtonSettings(button);
909932
_buttonActionArray[button]->deleteLater();
910933
_buttonActionArray[button] = nullptr;
911934
}
@@ -915,7 +938,7 @@ void Joystick::setButtonAction(int button, const QString& action)
915938
qCDebug(JoystickLog) << "setButtonAction: isPwmAction " << isPwmAction;
916939

917940
if (!_buttonActionArray[button]) {
918-
_buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1000, 2000, false)
941+
_buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1500, 1500, false)
919942
: new AssignedButtonAction(this, action);
920943
} else {
921944
if (isPwmAction) {
@@ -933,14 +956,14 @@ void Joystick::setButtonAction(int button, const QString& action)
933956
}
934957

935958
//-- Make sure repeat is off if this action doesn't support repeats
936-
setButtonRepeatIfAvailable(button, false);
937-
saveButtonSettings(button);
959+
_setButtonRepeatIfAvailable(button, false);
960+
_saveButtonSettings(button);
938961
}
939962
emit pwmVisibilitiesChanged();
940963
emit buttonActionsChanged();
941964
}
942965

943-
void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
966+
void Joystick::_setButtonRepeatIfAvailable(int button, bool repeat)
944967
{
945968
int idx = _findAssignableButtonAction(_buttonActionArray[button]->action());
946969
if (idx >= 0) {
@@ -953,7 +976,7 @@ void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
953976
_buttonActionArray[button]->repeat(false);
954977
}
955978

956-
void Joystick::saveButtonSettings(int button)
979+
void Joystick::_saveButtonSettings(int button)
957980
{
958981
QSettings settings;
959982
settings.beginGroup(_settingsGroup);
@@ -968,7 +991,7 @@ void Joystick::saveButtonSettings(int button)
968991
}
969992
}
970993

971-
void Joystick::removeButtonSettings(int button)
994+
void Joystick::_removeButtonSettings(int button)
972995
{
973996
QSettings settings;
974997
settings.beginGroup(_settingsGroup);
@@ -1347,10 +1370,10 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13471370
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
13481371
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
13491372

1350-
// TODO(bzd) take channel nos from config, especially max
1351-
for (int ch = 8; ch <= 16;ch++) {
1352-
_assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
1373+
for (int ch = 8; ch <= MAX_RC_CHANNELS; ch++) {
1374+
_assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
13531375
}
1376+
_clearRcOverrideButtonActions();
13541377

13551378
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
13561379
for (auto& item : _customMavCommands) {
@@ -1367,8 +1390,27 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13671390
bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown)
13681391
{
13691392
if (_buttonActionArray[buttonIndex]) {
1370-
_buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown);
1393+
uint8_t channel = _buttonActionArray[buttonIndex]->rcChannel();
1394+
auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(_activeVehicle, buttonDown);
1395+
_rcOverride[channel-1] = pwm;
1396+
_rcOverrideActive = true;
13711397
return true;
13721398
}
13731399
return false;
13741400
}
1401+
1402+
void Joystick::_clearRcOverrideButtonActions() {
1403+
qCDebug(JoystickLog) << "Clearing RC override button actions";
1404+
for (int i = 0; i < MAX_RC_CHANNELS; i++) {
1405+
_rcOverride[i] = -1;
1406+
}
1407+
}
1408+
1409+
uint16_t Joystick::_mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value) {
1410+
// UINT16_MAX - ignore
1411+
// 0 Release
1412+
if (value == 0) {
1413+
return rcChannel < 9 ? 0 : UINT16_MAX - 1;
1414+
}
1415+
return value;
1416+
}

0 commit comments

Comments
 (0)