Skip to content

Commit bb49814

Browse files
committed
Refactor PWM override to single joystick action class
1 parent 23f9870 commit bb49814

File tree

3 files changed

+98
-97
lines changed

3 files changed

+98
-97
lines changed

src/Joystick/Joystick.cc

Lines changed: 75 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -91,22 +91,31 @@ const float Joystick::_maxButtonFrequencyHz = 50.0f;
9191

9292
AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action)
9393
: QObject(parent)
94-
, action(action)
94+
, _action(action)
95+
, _isPwmOverrideAction(false)
96+
, _rcChannel(0)
9597
{
9698
}
9799

98-
AssignedButtonRcPwmOverrideAction::AssignedButtonRcPwmOverrideAction(
100+
AssignedButtonAction::AssignedButtonAction(
99101
QObject *parent, const QString action, const uint16_t loPwmValue, const uint16_t hiPwmValue, bool latch)
100-
: AssignedButtonAction(parent, action)
102+
: QObject(parent)
103+
, _action(action)
104+
, _isPwmOverrideAction(true)
101105
, _loPwmValue(loPwmValue)
102106
, _hiPwmValue(hiPwmValue)
103107
, _latchMode(latch)
104108
, _rcChannel(getRcChannelFromAction(action))
105109
{
106110
}
107111

108-
void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown)
112+
void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown)
109113
{
114+
if (!_isPwmOverrideAction) {
115+
qCWarning(JoystickLog) << "send called on non-pwm action";
116+
return;
117+
}
118+
110119
uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue;
111120
if (_latchMode) {
112121
qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_latchButtonDown ? "down" : "up");
@@ -124,22 +133,27 @@ void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown)
124133
vehicle->rcChannelOverride(_rcChannel, pwmValue);
125134
}
126135

127-
uint8_t AssignedButtonRcPwmOverrideAction::channel() const
136+
uint8_t AssignedButtonAction::pwmChannel() const
128137
{
129138
return _rcChannel;
130139
};
131140

132-
void AssignedButtonRcPwmOverrideAction::setLatchMode(bool latch)
141+
void AssignedButtonAction::pwmLatchMode(bool latch)
133142
{
134143
_latchMode = latch;
135144
}
136145

137-
bool AssignedButtonRcPwmOverrideAction::getLatchMode()
146+
bool AssignedButtonAction::pwmLatchMode() const
138147
{
139148
return _latchMode;
140149
}
141150

142-
uint8_t AssignedButtonRcPwmOverrideAction::getRcChannelFromAction(const QString action)
151+
bool AssignedButtonAction::isPwmOverrideAction() const
152+
{
153+
return _isPwmOverrideAction;
154+
}
155+
156+
uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action)
143157
{
144158
QRegularExpression re("^Channel (\\d{1,2}) direct PWM");
145159
QRegularExpressionMatch match = re.match(action);
@@ -357,10 +371,6 @@ void Joystick::_loadSettings()
357371
// FunctionAxis mappings are always stored in TX mode 2
358372
// Remap to stored TX mode in settings
359373
_remapAxes(2, _transmitterMode, _rgFunctionAxis);
360-
//TODO add with real channels
361-
// for (int channel = 8; channel <= 16; channel++) {
362-
// _rcOverrides.append(nullptr);
363-
// }
364374

365375
for (int button = 0; button < _totalButtonCount; button++) {
366376
QString action = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString();
@@ -376,21 +386,16 @@ void Joystick::_loadSettings()
376386
bool latch = settings.value(QString(_buttonActionLatchPwmValueKey).arg(button), false).toBool();
377387
qCDebug(JoystickLog) << "_loadSettings button:action:pwm" << button << action << "L:" << lowPwm
378388
<< " H:" << highPwm << " Latch:" << latch;
379-
// int idx = channel - 8;
380-
// if (_rcOverrides[idx]) {
381-
// delete _rcOverrides[idx];
382-
// }
383-
ap = new AssignedButtonRcPwmOverrideAction(this, action, lowPwm, highPwm, false);
384-
//.append(JoystickRcOverride(channel, lowPwm, highPwm, latch));
389+
ap = new AssignedButtonAction(this, action, lowPwm, highPwm, latch);
385390
} else {
386391
ap = new AssignedButtonAction(this, action);
387-
ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool();
388392
}
393+
ap->repeat(settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool());
389394

390395
_buttonActionArray[button] = ap;
391-
_buttonActionArray[button]->buttonTime.start();
392-
qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action
393-
<< _buttonActionArray[button]->repeat;
396+
_buttonActionArray[button]->buttonTime().start();
397+
qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action()
398+
<< _buttonActionArray[button]->repeat();
394399

395400
}
396401
}
@@ -408,11 +413,14 @@ void Joystick::_saveButtonSettings()
408413
settings.beginGroup(_name);
409414
for (int button = 0; button < _totalButtonCount; button++) {
410415
if(_buttonActionArray[button]) {
411-
settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action);
412-
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
413-
qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat;
414-
if (assignableActionIsPwm(button)) {
415-
//TODO finish, but low priority
416+
settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action());
417+
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat());
418+
qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action() << _buttonActionArray[button]->repeat();
419+
if (_buttonActionArray[button]->isPwmOverrideAction()) {
420+
settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm());
421+
settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm());
422+
settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode());
423+
416424
}
417425
}
418426
}
@@ -570,7 +578,7 @@ void Joystick::run()
570578
_axisTime.start();
571579
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
572580
if(_buttonActionArray[buttonIndex]) {
573-
_buttonActionArray[buttonIndex]->buttonTime.start();
581+
_buttonActionArray[buttonIndex]->buttonTime().start();
574582
}
575583
}
576584
while (!_exitThread) {
@@ -619,18 +627,18 @@ void Joystick::_handleButtons()
619627
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
620628
if(_rgButtonValues[buttonIndex] == BUTTON_DOWN || _rgButtonValues[buttonIndex] == BUTTON_REPEAT) {
621629
if(_buttonActionArray[buttonIndex]) {
622-
QString buttonAction = _buttonActionArray[buttonIndex]->action;
630+
QString buttonAction = _buttonActionArray[buttonIndex]->action();
623631
if(buttonAction.isEmpty() || buttonAction == _buttonActionNone)
624632
continue;
625-
if(!_buttonActionArray[buttonIndex]->repeat) {
633+
if(!_buttonActionArray[buttonIndex]->repeat()) {
626634
//-- This button just went down
627635
if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) {
628636
// Check for a multi-button action
629637
QList<int> rgButtons = { buttonIndex };
630638
bool executeButtonAction = true;
631639
for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) {
632640
if (multiIndex != buttonIndex) {
633-
if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) {
641+
if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action() == buttonAction) {
634642
// We found a multi-button action
635643
if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) {
636644
// So far so good
@@ -652,8 +660,8 @@ void Joystick::_handleButtons()
652660
} else {
653661
//-- Process repeat buttons
654662
int buttonDelay = static_cast<int>(1000.0f / _buttonFrequencyHz);
655-
if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) {
656-
_buttonActionArray[buttonIndex]->buttonTime.start();
663+
if(_buttonActionArray[buttonIndex]->buttonTime().elapsed() > buttonDelay) {
664+
_buttonActionArray[buttonIndex]->buttonTime().start();
657665
qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction;
658666
_executeButtonAction(buttonAction, buttonIndex, true);
659667
}
@@ -666,7 +674,7 @@ void Joystick::_handleButtons()
666674
if(buttonIndex < 256) {
667675
if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) {
668676
if(_buttonActionArray[buttonIndex]) {
669-
QString buttonAction = _buttonActionArray[buttonIndex]->action;
677+
QString buttonAction = _buttonActionArray[buttonIndex]->action();
670678
if(buttonAction.isEmpty() || buttonAction == _buttonActionNone)
671679
continue;
672680
qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction;
@@ -879,21 +887,21 @@ void Joystick::setButtonRepeat(int button, bool repeat)
879887
if (!_validButton(button) || !_buttonActionArray[button]) {
880888
return;
881889
}
882-
_buttonActionArray[button]->repeat = repeat;
883-
_buttonActionArray[button]->buttonTime.start();
890+
_buttonActionArray[button]->repeat(repeat);
891+
_buttonActionArray[button]->buttonTime().start();
884892
//-- Save to settings
885893
QSettings settings;
886894
settings.beginGroup(_settingsGroup);
887895
settings.beginGroup(_name);
888-
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
896+
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat());
889897
}
890898

891899
bool Joystick::getButtonRepeat(int button)
892900
{
893901
if (!_validButton(button) || !_buttonActionArray[button]) {
894902
return false;
895903
}
896-
return _buttonActionArray[button]->repeat;
904+
return _buttonActionArray[button]->repeat();
897905
}
898906

899907
void Joystick::setButtonAction(int button, const QString& action)
@@ -907,20 +915,24 @@ void Joystick::setButtonAction(int button, const QString& action)
907915
settings.beginGroup(_name);
908916
if(action.isEmpty() || action == _buttonActionNone) {
909917
if(_buttonActionArray[button]) {
910-
_buttonActionArray[button]->deleteLater();
911-
_buttonActionArray[button] = nullptr;
912918
//-- Clear from settings
913-
//TODO clear if PWM type
914919
settings.remove(QString(_buttonActionNameKey).arg(button));
915920
settings.remove(QString(_buttonActionRepeatKey).arg(button));
921+
if (_buttonActionArray[button]->isPwmOverrideAction()) {
922+
settings.remove(QString(_buttonActionHighPwmValueKey).arg(button));
923+
settings.remove(QString(_buttonActionLowPwmValueKey).arg(button));
924+
settings.remove(QString(_buttonActionLatchPwmValueKey).arg(button));
925+
}
926+
_buttonActionArray[button]->deleteLater();
927+
_buttonActionArray[button] = nullptr;
916928
}
917929
} else {
918930
bool isPwmAction = assignableActionIsPwm(action);
919931
AssignedButtonAction *ap;
920932

921933
if(!_buttonActionArray[button]) {
922934
if (isPwmAction) {
923-
ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false);
935+
ap = new AssignedButtonAction(this, action, 0, 0, false);
924936
} else {
925937
ap = new AssignedButtonAction(this, action);
926938
}
@@ -929,23 +941,28 @@ void Joystick::setButtonAction(int button, const QString& action)
929941
if (isPwmAction) {
930942
//TODO fix it better, maybe one aciton with PWM values in it
931943
_buttonActionArray[button]->deleteLater();
932-
ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false);
944+
ap = new AssignedButtonAction(this, action, 0, 0, false);
933945
_buttonActionArray[button] = ap;
934946
}
935-
_buttonActionArray[button]->action = action;
947+
_buttonActionArray[button]->action(action);
936948
}
937949
//-- Make sure repeat is off if this action doesn't support repeats
938950
int idx = _findAssignableButtonAction(action);
939951
if(idx >= 0) {
940952
AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[idx]);
941953
if(!p->canRepeat()) {
942-
_buttonActionArray[button]->repeat = false;
954+
_buttonActionArray[button]->repeat(false);
943955
}
944956
}
945957

946958
//-- Save to settings
947-
settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action);
948-
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
959+
settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action());
960+
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat());
961+
if (isPwmAction) {
962+
settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm());
963+
settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm());
964+
settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode());
965+
}
949966
}
950967
emit buttonActionsChanged();
951968
}
@@ -954,7 +971,7 @@ QString Joystick::getButtonAction(int button)
954971
{
955972
if (_validButton(button)) {
956973
if(_buttonActionArray[button]) {
957-
return _buttonActionArray[button]->action;
974+
return _buttonActionArray[button]->action();
958975
}
959976
}
960977
return QString(_buttonActionNone);
@@ -975,6 +992,7 @@ bool Joystick::assignableActionIsPwm(QString action) {
975992
}
976993

977994
void Joystick::setButtonPwm(int button, bool lowPwm, int value) {
995+
//TODO czy tutaj przychodzi z GUI i trzeba ustawic tez w _buttonActionArray[button]?
978996
qDebug(JoystickLog) << "setButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ") << value;
979997
if (_validButton(button)) {
980998
if (assignableActionIsPwm(button)) {
@@ -994,8 +1012,6 @@ int Joystick::getButtonPwm(int button, bool lowPwm) {
9941012
qDebug(JoystickLog) << "getButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ");
9951013
if (_validButton(button)) {
9961014
if (assignableActionIsPwm(button)) {
997-
// const AssignedButtonRcPwmOverrideAction *ap = dynamic_cast<const AssignedButtonRcPwmOverrideAction *>(_buttonActionArray[button]);
998-
// int rc = ap->channel();
9991015
QSettings settings;
10001016
settings.beginGroup(_settingsGroup);
10011017
settings.beginGroup(_name);
@@ -1016,25 +1032,24 @@ void Joystick::setButtonPwmLatch(int button, bool latch)
10161032
}
10171033
qCDebug(JoystickLog) << "PWM Latch mode for button " << button << (latch ? " enabled" : " disabled");
10181034
//TODO check is really this class
1019-
AssignedButtonRcPwmOverrideAction *action = dynamic_cast<AssignedButtonRcPwmOverrideAction *>(_buttonActionArray[button]);
1020-
action->setLatchMode(latch);
1035+
auto *action = (_buttonActionArray[button]);
1036+
action->pwmLatchMode(latch);
10211037

10221038
//TODO finish saving settings
1023-
/*
1039+
10241040
QSettings settings;
10251041
settings.beginGroup(_settingsGroup);
10261042
settings.beginGroup(_name);
1027-
settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
1028-
*/
1043+
settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), action->pwmLatchMode());
10291044
}
10301045

10311046
bool Joystick::getButtonPwmLatch(int button) {
10321047
if (!_validButton(button) || !_buttonActionArray[button]) {
10331048
return false;
10341049
}
10351050
//TODO check is really this class
1036-
AssignedButtonRcPwmOverrideAction *action = dynamic_cast<AssignedButtonRcPwmOverrideAction *>(_buttonActionArray[button]);
1037-
return action->getLatchMode();
1051+
auto *action = (_buttonActionArray[button]);
1052+
return action->pwmLatchMode();
10381053
}
10391054

10401055
QStringList Joystick::buttonActions()
@@ -1328,6 +1343,7 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13281343
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
13291344
_assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
13301345

1346+
// TODO(bzd) take channel nos from config, especially max
13311347
for (int ch = 8; ch <= 16;ch++) {
13321348
_assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
13331349
}
@@ -1347,23 +1363,8 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
13471363
bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown)
13481364
{
13491365
if (_buttonActionArray[buttonIndex]) {
1350-
AssignedButtonRcPwmOverrideAction* buttonAction =
1351-
dynamic_cast<AssignedButtonRcPwmOverrideAction *>(_buttonActionArray[buttonIndex]);
1352-
buttonAction->send(_activeVehicle, buttonDown);
1366+
_buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown);
1367+
return true;
13531368
}
13541369
return false;
1355-
/* int channelNo = getRcChannelFromAction(action);
1356-
if (channelNo == -1) {
1357-
return false;
1358-
}
1359-
qCDebug(JoystickLog) << "RC override matched for channel " << channelNo;
1360-
//TODO make a map rcChannel : override (ev null if not set)
1361-
for (auto& item : _rcOverrides) {
1362-
if (channelNo == item->channel()) {
1363-
qCDebug(JoystickLog) << "Sending RC override " << channelNo;
1364-
item->send(_activeVehicle, buttonDown);
1365-
return true;
1366-
}
1367-
}
1368-
return false;*/
13691370
}

0 commit comments

Comments
 (0)