Skip to content

Commit e662fe9

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

14 files changed

+155
-50
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-25
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)
@@ -162,11 +162,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
162162
, _totalButtonCount(_buttonCount+_hatButtonCount)
163163
, _multiVehicleManager(multiVehicleManager)
164164
{
165-
<<<<<<< HEAD
166165
qRegisterMetaType<GRIPPER_ACTIONS>();
167166

168-
=======
169-
>>>>>>> Refactor for clearness
170167
_rgAxisValues = new int[static_cast<size_t>(_axisCount)];
171168
_rgCalibration = new Calibration_t[static_cast<size_t>(_axisCount)];
172169
_rgButtonValues = new uint8_t[static_cast<size_t>(_totalButtonCount)];
@@ -177,6 +174,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
177174
_rgButtonValues[i] = BUTTON_UP;
178175
_buttonActionArray.append(nullptr);
179176
}
177+
180178
_buildActionList(_multiVehicleManager->activeVehicle());
181179
_updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
182180
_loadSettings();
@@ -277,6 +275,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
277275
int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
278276
setTXMode(mode);
279277
}
278+
_clearRcOverrideButtonActions();
280279
}
281280

282281
void Joystick::_loadSettings()
@@ -384,7 +383,7 @@ void Joystick::_loadSettings()
384383
_buttonActionArray[button]->buttonTime().start();
385384

386385
bool savedRepeatState = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool();
387-
setButtonRepeatIfAvailable(button, savedRepeatState);
386+
_setButtonRepeatIfAvailable(button, savedRepeatState);
388387

389388
qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action()
390389
<< _buttonActionArray[button]->repeat();
@@ -411,7 +410,6 @@ void Joystick::_saveButtonSettings()
411410
settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm());
412411
settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm());
413412
settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode());
414-
415413
}
416414
}
417415
}
@@ -567,6 +565,7 @@ void Joystick::run()
567565
_open();
568566
//-- Reset timers
569567
_axisTime.start();
568+
_rcOverrideTimer.start();
570569
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
571570
if(_buttonActionArray[buttonIndex]) {
572571
_buttonActionArray[buttonIndex]->buttonTime().start();
@@ -576,6 +575,7 @@ void Joystick::run()
576575
_update();
577576
_handleButtons();
578577
_handleAxis();
578+
_handleRcOverride();
579579
QGC::SLEEP::msleep(qMin(static_cast<int>(1000.0f / _maxAxisFrequencyHz), static_cast<int>(1000.0f / _maxButtonFrequencyHz)) / 2);
580580
}
581581
_close();
@@ -773,6 +773,26 @@ void Joystick::_handleAxis()
773773
}
774774
}
775775

776+
void Joystick::_handleRcOverride()
777+
{
778+
if (!_activeVehicle || !_activeVehicle->supportsRcChannelOverride()) {
779+
return;
780+
}
781+
const int rcOverrideInterval = static_cast<int>(1000.0f / _rcOverrideFrequencyHz);
782+
783+
if (_rcOverrideActive || _rcOverrideTimer.elapsed() > rcOverrideInterval) {
784+
_rcOverrideActive = false;
785+
_rcOverrideTimer.start();
786+
uint16_t overrideData[18] = {UINT16_MAX};
787+
overrideData[16] = _mapRcOverrideToRelease(17, 0);
788+
overrideData[17] = _mapRcOverrideToRelease(18, 0);
789+
for (int i = 0; i < MAX_RC_CHANNELS; i++) {
790+
overrideData[i] = _rcOverride[i] == -1 ? _mapRcOverrideToRelease(i + 1, 0) : _rcOverride[i];
791+
}
792+
_activeVehicle->rcChannelsOverride(overrideData);
793+
}
794+
}
795+
776796
void Joystick::startPolling(Vehicle* vehicle)
777797
{
778798
if (vehicle) {
@@ -905,7 +925,7 @@ void Joystick::setButtonAction(int button, const QString& action)
905925
if (action.isEmpty() || action == _buttonActionNone) {
906926
_pwmSettingsVisibilities[button] = false;
907927
if (_buttonActionArray[button]) {
908-
removeButtonSettings(button);
928+
_removeButtonSettings(button);
909929
_buttonActionArray[button]->deleteLater();
910930
_buttonActionArray[button] = nullptr;
911931
}
@@ -915,7 +935,7 @@ void Joystick::setButtonAction(int button, const QString& action)
915935
qCDebug(JoystickLog) << "setButtonAction: isPwmAction " << isPwmAction;
916936

917937
if (!_buttonActionArray[button]) {
918-
_buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1000, 2000, false)
938+
_buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1500, 1500, false)
919939
: new AssignedButtonAction(this, action);
920940
} else {
921941
if (isPwmAction) {
@@ -933,14 +953,14 @@ void Joystick::setButtonAction(int button, const QString& action)
933953
}
934954

935955
//-- Make sure repeat is off if this action doesn't support repeats
936-
setButtonRepeatIfAvailable(button, false);
937-
saveButtonSettings(button);
956+
_setButtonRepeatIfAvailable(button, false);
957+
_saveButtonSettings(button);
938958
}
939959
emit pwmVisibilitiesChanged();
940960
emit buttonActionsChanged();
941961
}
942962

943-
void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
963+
void Joystick::_setButtonRepeatIfAvailable(int button, bool repeat)
944964
{
945965
int idx = _findAssignableButtonAction(_buttonActionArray[button]->action());
946966
if (idx >= 0) {
@@ -953,7 +973,7 @@ void Joystick::setButtonRepeatIfAvailable(int button, bool repeat)
953973
_buttonActionArray[button]->repeat(false);
954974
}
955975

956-
void Joystick::saveButtonSettings(int button)
976+
void Joystick::_saveButtonSettings(int button)
957977
{
958978
QSettings settings;
959979
settings.beginGroup(_settingsGroup);
@@ -968,7 +988,7 @@ void Joystick::saveButtonSettings(int button)
968988
}
969989
}
970990

971-
void Joystick::removeButtonSettings(int button)
991+
void Joystick::_removeButtonSettings(int button)
972992
{
973993
QSettings settings;
974994
settings.beginGroup(_settingsGroup);
@@ -1347,10 +1367,10 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13471367
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
13481368
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
13491369

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)));
1370+
for (int ch = 8; ch <= MAX_RC_CHANNELS; ch++) {
1371+
_assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
13531372
}
1373+
_clearRcOverrideButtonActions();
13541374

13551375
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
13561376
for (auto& item : _customMavCommands) {
@@ -1367,8 +1387,27 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13671387
bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown)
13681388
{
13691389
if (_buttonActionArray[buttonIndex]) {
1370-
_buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown);
1390+
uint8_t channel = _buttonActionArray[buttonIndex]->rcChannel();
1391+
auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(_activeVehicle, buttonDown);
1392+
_rcOverride[channel-1] = pwm;
1393+
_rcOverrideActive = true;
13711394
return true;
13721395
}
13731396
return false;
13741397
}
1398+
1399+
void Joystick::_clearRcOverrideButtonActions() {
1400+
qCDebug(JoystickLog) << "Clearing RC override button actions";
1401+
for (int i = 0; i < MAX_RC_CHANNELS; i++) {
1402+
_rcOverride[i] = -1;
1403+
}
1404+
}
1405+
1406+
uint16_t Joystick::_mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value) {
1407+
// UINT16_MAX - ignore
1408+
// 0 Release
1409+
if (value == 0) {
1410+
return rcChannel < 9 ? 0 : UINT16_MAX - 1;
1411+
}
1412+
return value;
1413+
}

0 commit comments

Comments
 (0)