Skip to content

Commit 4929090

Browse files
zdanekzdanek
committed
Add Saving PWM values for button RC override
1 parent 3e6b4c2 commit 4929090

File tree

9 files changed

+383
-53
lines changed

9 files changed

+383
-53
lines changed

‎qgroundcontrol.qrc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@
181181
<file alias="QGroundControl/Controls/TerrainStatus.qml">src/PlanView/TerrainStatus.qml</file>
182182
<file alias="QGroundControl/Controls/TakeoffItemMapVisual.qml">src/PlanView/TakeoffItemMapVisual.qml</file>
183183
<file alias="QGroundControl/Controls/ToolStrip.qml">src/QmlControls/ToolStrip.qml</file>
184-
<file alias="QGroundControl/Controls/ToolStripHoverButton.qml">src/QmlControls/ToolStripHoverButton.qml</file>
184+
<file alias="QGroundControl/Controls/ToolStripHoverButton.qml">src/QmlControls/ToolStripHoverButton.qml</file>
185185
<file alias="QGroundControl/Controls/TransectStyleComplexItemEditor.qml">src/PlanView/TransectStyleComplexItemEditor.qml</file>
186186
<file alias="QGroundControl/Controls/TransectStyleComplexItemStats.qml">src/PlanView/TransectStyleComplexItemStats.qml</file>
187187
<file alias="QGroundControl/Controls/TransectStyleComplexItemTabBar.qml">src/PlanView/TransectStyleComplexItemTabBar.qml</file>

‎src/Joystick/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ add_library(Joystick
1212
JoystickManager.cc
1313
JoystickSDL.cc
1414
JoystickMavCommand.cc
15+
JoystickRcOverride.cc
1516
${EXTRA_SRC}
1617
)
1718

‎src/Joystick/Joystick.cc

Lines changed: 107 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ const char* Joystick::_settingsGroup = "Joysticks";
2626
const char* Joystick::_calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration
2727
const char* Joystick::_buttonActionNameKey = "ButtonActionName%1";
2828
const char* Joystick::_buttonActionRepeatKey = "ButtonActionRepeat%1";
29+
const char* Joystick::_buttonActionLowPwmValueKey = "ButtonActionLowPwm%1";
30+
const char* Joystick::_buttonActionHighPwmValueKey = "ButtonActionHighPwm%1";
2931
const char* Joystick::_throttleModeSettingsKey = "ThrottleMode";
3032
const char* Joystick::_negativeThrustSettingsKey = "NegativeThrust";
3133
const char* Joystick::_exponentialSettingsKey = "Exponential";
@@ -126,6 +128,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
126128
connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
127129

128130
_customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json");
131+
JoystickRcOverride rco(8, 1200, 1800, true);
132+
_rcOverrides.append(rco);
129133
}
130134

131135
void Joystick::stop()
@@ -858,6 +862,59 @@ QString Joystick::getButtonAction(int button)
858862
return QString(_buttonActionNone);
859863
}
860864

865+
bool Joystick::assignableActionIsPWM(int button) {
866+
qDebug(JoystickLog) << "assignableActionIsPWM: " << button;
867+
if (_validButton(button)) {
868+
auto action = getButtonAction(button);
869+
qDebug(JoystickLog) << "action " << action;
870+
return action.contains("PWM");
871+
}
872+
return false;
873+
}
874+
875+
void Joystick::setButtonPWM(int button, bool lowPwm, int value) {
876+
qDebug(JoystickLog) << "setButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ") << value;
877+
if (_validButton(button)) {
878+
if (assignableActionIsPWM(button)) {
879+
auto action = getButtonAction(button);
880+
int rc = getRcChannelFromAction(action);
881+
if (rc < 0) {
882+
return;
883+
}
884+
QSettings settings;
885+
settings.beginGroup(_settingsGroup);
886+
settings.beginGroup(_name);
887+
if (lowPwm) {
888+
settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value);
889+
} else {
890+
settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value);
891+
}
892+
}
893+
}
894+
}
895+
896+
int Joystick::getButtonPWM(int button, bool lowPwm) {
897+
qDebug(JoystickLog) << "getButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ");
898+
if (_validButton(button)) {
899+
if (assignableActionIsPWM(button)) {
900+
auto action = getButtonAction(button);
901+
int rc = getRcChannelFromAction(action);
902+
if (rc < 0) {
903+
return -1;
904+
}
905+
QSettings settings;
906+
settings.beginGroup(_settingsGroup);
907+
settings.beginGroup(_name);
908+
if (lowPwm) {
909+
return settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt();
910+
} else {
911+
return settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt();
912+
}
913+
}
914+
}
915+
return -1;
916+
}
917+
861918
QStringList Joystick::buttonActions()
862919
{
863920
QStringList list;
@@ -1043,13 +1100,22 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown)
10431100
emit gripperAction(GRIPPER_ACTION_RELEASE);
10441101
}
10451102
} else {
1046-
if (buttonDown && _activeVehicle) {
1047-
for (auto& item : _customMavCommands) {
1048-
if (action == item.name()) {
1049-
item.send(_activeVehicle);
1050-
return;
1103+
qCDebug(JoystickLog) << "Action " << action << " button " << (buttonDown ? "down" : "up");
1104+
if (_activeVehicle) {
1105+
bool actionServed = false;
1106+
if (action.startsWith("Channel")) {
1107+
actionServed = _executeRcOverrideButtonAction(action, buttonDown);
1108+
}
1109+
if (buttonDown && !actionServed) {
1110+
for (auto& item : _customMavCommands) {
1111+
if (action == item.name()) {
1112+
item.send(_activeVehicle);
1113+
return;
1114+
}
10511115
}
10521116
}
1117+
} else {
1118+
qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
10531119
}
10541120
}
10551121
}
@@ -1141,6 +1207,11 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
11411207
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
11421208
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
11431209

1210+
for (int ch = 8; ch <= 16;ch++) {
1211+
_assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
1212+
}
1213+
1214+
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
11441215
for (auto& item : _customMavCommands) {
11451216
_assignableButtonActions.append(new AssignableButtonAction(this, item.name()));
11461217
}
@@ -1151,3 +1222,34 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
11511222
}
11521223
emit assignableActionsChanged();
11531224
}
1225+
1226+
bool Joystick::_executeRcOverrideButtonAction(const QString &action, bool buttonDown)
1227+
{
1228+
QRegularExpression re("^Channel (\\d{1,2}) direct PWM");
1229+
QRegularExpressionMatch match = re.match(action);
1230+
if (match.hasMatch()) {
1231+
QString channelNoStr = match.captured(1);
1232+
qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr;
1233+
uint8_t channelNo = channelNoStr.toInt();
1234+
1235+
for (auto& item : _rcOverrides) {
1236+
if (channelNo == item.channel()) {
1237+
item.send(_activeVehicle, buttonDown);
1238+
return true;
1239+
}
1240+
}
1241+
}
1242+
return false;
1243+
}
1244+
1245+
int Joystick::getRcChannelFromAction(QString actionName) {
1246+
QRegularExpression re("^Channel (\\d{1,2}) direct PWM");
1247+
QRegularExpressionMatch match = re.match(actionName);
1248+
if (match.hasMatch()) {
1249+
QString channelNoStr = match.captured(1);
1250+
qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr;
1251+
uint8_t channelNo = channelNoStr.toInt();
1252+
return channelNo;
1253+
}
1254+
return -1;
1255+
}

‎src/Joystick/Joystick.h

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,11 @@
1616
#include <QThread>
1717
#include <atomic>
1818

19+
#include "JoystickMavCommand.h"
20+
#include "JoystickRcOverride.h"
21+
#include "MultiVehicleManager.h"
1922
#include "QGCLoggingCategory.h"
2023
#include "Vehicle.h"
21-
#include "MultiVehicleManager.h"
22-
#include "JoystickMavCommand.h"
2324

2425
Q_DECLARE_LOGGING_CATEGORY(JoystickLog)
2526
Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog)
@@ -118,6 +119,10 @@ class Joystick : public QThread
118119
Q_INVOKABLE bool getButtonRepeat (int button);
119120
Q_INVOKABLE void setButtonAction (int button, const QString& action);
120121
Q_INVOKABLE QString getButtonAction (int button);
122+
Q_INVOKABLE bool assignableActionIsPWM (int button);
123+
Q_INVOKABLE void setButtonPWM (int button, bool lowPwm, int value);
124+
Q_INVOKABLE int getButtonPWM (int button, bool lowPwm);
125+
121126

122127
// Property accessors
123128

@@ -307,6 +312,7 @@ class Joystick : public QThread
307312
MultiVehicleManager* _multiVehicleManager = nullptr;
308313

309314
QList<JoystickMavCommand> _customMavCommands;
315+
QList<JoystickRcOverride> _rcOverrides;
310316

311317
static const float _minAxisFrequencyHz;
312318
static const float _maxAxisFrequencyHz;
@@ -320,6 +326,8 @@ class Joystick : public QThread
320326
static const char* _calibratedSettingsKey;
321327
static const char* _buttonActionNameKey;
322328
static const char* _buttonActionRepeatKey;
329+
static const char* _buttonActionLowPwmValueKey;
330+
static const char* _buttonActionHighPwmValueKey;
323331
static const char* _throttleModeSettingsKey;
324332
static const char* _negativeThrustSettingsKey;
325333
static const char* _exponentialSettingsKey;
@@ -365,4 +373,6 @@ class Joystick : public QThread
365373

366374
private slots:
367375
void _activeVehicleChanged(Vehicle* activeVehicle);
376+
bool _executeRcOverrideButtonAction(const QString &action, bool buttonDown);
377+
int getRcChannelFromAction(QString actionName);
368378
};

‎src/Joystick/JoystickRcOverride.cc

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
//
2+
// Created by zdanek on 08.07.2022.
3+
//
4+
5+
#include "JoystickRcOverride.h"
6+
7+
#include "Vehicle.h"
8+
9+
JoystickRcOverride::JoystickRcOverride(const uint8_t rcChannel,
10+
const uint16_t loPwmValue,
11+
const uint16_t hiPwmValue,
12+
bool latch)
13+
: rcChannel(rcChannel)
14+
, loPwmValue(loPwmValue)
15+
, hiPwmValue(hiPwmValue)
16+
, latchMode(latch)
17+
{}
18+
19+
void JoystickRcOverride::send(Vehicle *vehicle, bool buttonDown)
20+
{
21+
uint16_t pwmValue = buttonDown ? hiPwmValue : loPwmValue;
22+
23+
if (latchMode) {
24+
if (buttonDown) {
25+
latchButtonDown = !latchButtonDown;
26+
pwmValue = latchButtonDown ? hiPwmValue : loPwmValue;
27+
} else {
28+
return;
29+
}
30+
}
31+
vehicle->rcChannelOverride(rcChannel, pwmValue);
32+
}
33+
34+
uint8_t JoystickRcOverride::channel() const
35+
{
36+
return rcChannel;
37+
};

‎src/Joystick/JoystickRcOverride.h

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
//
2+
// Created by zdanek on 08.07.2022.
3+
//
4+
5+
#pragma once
6+
7+
#include "Vehicle.h"
8+
9+
class JoystickRcOverride
10+
{
11+
public:
12+
JoystickRcOverride(const uint8_t rcChannel,
13+
const uint16_t loPwmValue,
14+
const uint16_t hiPwmValue,
15+
bool latch);
16+
void send(Vehicle* vehicle, bool buttonDown);
17+
uint8_t channel() const;
18+
19+
private:
20+
uint8_t rcChannel;
21+
uint16_t loPwmValue;
22+
uint16_t hiPwmValue;
23+
bool latchMode;
24+
bool latchButtonDown = false;
25+
};
26+

‎src/Vehicle/Vehicle.cc

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4188,3 +4188,62 @@ void Vehicle::setGripperAction(GRIPPER_ACTIONS gripperAction)
41884188
gripperAction, // Param2: Gripper Action
41894189
0, 0, 0, 0, 0); // Param 3 ~ 7 : unused
41904190
}
4191+
4192+
void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue)
4193+
{
4194+
const int maxRcChannels = 16;
4195+
if (rcChannel > maxRcChannels) {
4196+
qCWarning(VehicleLog) << "Unsupported rc channel " << rcChannel << " to override";
4197+
return;
4198+
}
4199+
if (pwmValue > 2000 || pwmValue < 1000) {
4200+
qCWarning(VehicleLog) << "Bad PWM override value " << pwmValue << " for channel " << rcChannel;
4201+
return;
4202+
}
4203+
SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock();
4204+
if (!sharedLink) {
4205+
qCDebug(VehicleLog)<< "rcChannelOverride: primary link gone!";
4206+
return;
4207+
}
4208+
4209+
if (sharedLink->linkConfiguration()->isHighLatency()) {
4210+
return;
4211+
}
4212+
4213+
qCDebug(VehicleLog) << "Sending RC channel " << rcChannel << " PWM override to " << pwmValue;
4214+
4215+
uint16_t override_data[18] = {};
4216+
for (int i = 0; i < 18; i++) {
4217+
override_data[i] = UINT16_MAX;
4218+
}
4219+
override_data[rcChannel - 1] = pwmValue;
4220+
4221+
mavlink_message_t message;
4222+
4223+
mavlink_msg_rc_channels_override_pack_chan(
4224+
static_cast<uint8_t>(_mavlink->getSystemId()),
4225+
static_cast<uint8_t>(_mavlink->getComponentId()),
4226+
sharedLink->mavlinkChannel(),
4227+
&message,
4228+
static_cast<uint8_t>(_id),
4229+
0,
4230+
override_data[0],
4231+
override_data[1],
4232+
override_data[2],
4233+
override_data[3],
4234+
override_data[4],
4235+
override_data[5],
4236+
override_data[6],
4237+
override_data[7],
4238+
override_data[8],
4239+
override_data[9],
4240+
override_data[10],
4241+
override_data[11],
4242+
override_data[12],
4243+
override_data[13],
4244+
override_data[14],
4245+
override_data[15],
4246+
override_data[16],
4247+
override_data[17]);
4248+
sendMessageOnLinkThreadSafe(sharedLink.get(), message);
4249+
}

‎src/Vehicle/Vehicle.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -479,6 +479,14 @@ class Vehicle : public FactGroup
479479
void setJoystickEnabled (bool enabled);
480480
void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons);
481481

482+
/// Sends Channel override value
483+
/// @param rcChannel channel number 1-16
484+
/// @param pwmValue direct value 1000 - 2000
485+
void rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue);
486+
/// Sends disabling of channel override
487+
/// @param rcChannel channel number 1-16
488+
void disableChannelOverride(uint8_t rcChannel);
489+
482490
// Property accesors
483491
int id() const{ return _id; }
484492
MAV_AUTOPILOT firmwareType() const { return _firmwareType; }

0 commit comments

Comments
 (0)