@@ -91,22 +91,31 @@ const float Joystick::_maxButtonFrequencyHz = 50.0f;
91
91
92
92
AssignedButtonAction::AssignedButtonAction (QObject* parent, const QString action)
93
93
: QObject(parent)
94
- , action(action)
94
+ , _action(action)
95
+ , _isPwmOverrideAction(false )
96
+ , _rcChannel(0 )
95
97
{
96
98
}
97
99
98
- AssignedButtonRcPwmOverrideAction::AssignedButtonRcPwmOverrideAction (
100
+ AssignedButtonAction::AssignedButtonAction (
99
101
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 )
101
105
, _loPwmValue(loPwmValue)
102
106
, _hiPwmValue(hiPwmValue)
103
107
, _latchMode(latch)
104
108
, _rcChannel(getRcChannelFromAction(action))
105
109
{
106
110
}
107
111
108
- void AssignedButtonRcPwmOverrideAction::send (Vehicle *vehicle, bool buttonDown)
112
+ void AssignedButtonAction::sendPwm (Vehicle *vehicle, bool buttonDown)
109
113
{
114
+ if (!_isPwmOverrideAction) {
115
+ qCWarning (JoystickLog) << " send called on non-pwm action" ;
116
+ return ;
117
+ }
118
+
110
119
uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue;
111
120
if (_latchMode) {
112
121
qCDebug (JoystickLog) << " Latch mode, current saved button state " << (_latchButtonDown ? " down" : " up" );
@@ -124,22 +133,27 @@ void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown)
124
133
vehicle->rcChannelOverride (_rcChannel, pwmValue);
125
134
}
126
135
127
- uint8_t AssignedButtonRcPwmOverrideAction::channel () const
136
+ uint8_t AssignedButtonAction::pwmChannel () const
128
137
{
129
138
return _rcChannel;
130
139
};
131
140
132
- void AssignedButtonRcPwmOverrideAction::setLatchMode (bool latch)
141
+ void AssignedButtonAction::pwmLatchMode (bool latch)
133
142
{
134
143
_latchMode = latch;
135
144
}
136
145
137
- bool AssignedButtonRcPwmOverrideAction::getLatchMode ()
146
+ bool AssignedButtonAction::pwmLatchMode () const
138
147
{
139
148
return _latchMode;
140
149
}
141
150
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)
143
157
{
144
158
QRegularExpression re (" ^Channel (\\ d{1,2}) direct PWM" );
145
159
QRegularExpressionMatch match = re.match (action);
@@ -357,10 +371,6 @@ void Joystick::_loadSettings()
357
371
// FunctionAxis mappings are always stored in TX mode 2
358
372
// Remap to stored TX mode in settings
359
373
_remapAxes (2 , _transmitterMode, _rgFunctionAxis);
360
- // TODO add with real channels
361
- // for (int channel = 8; channel <= 16; channel++) {
362
- // _rcOverrides.append(nullptr);
363
- // }
364
374
365
375
for (int button = 0 ; button < _totalButtonCount; button++) {
366
376
QString action = settings.value (QString (_buttonActionNameKey).arg (button), QString ()).toString ();
@@ -376,21 +386,16 @@ void Joystick::_loadSettings()
376
386
bool latch = settings.value (QString (_buttonActionLatchPwmValueKey).arg (button), false ).toBool ();
377
387
qCDebug (JoystickLog) << " _loadSettings button:action:pwm" << button << action << " L:" << lowPwm
378
388
<< " 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);
385
390
} else {
386
391
ap = new AssignedButtonAction (this , action);
387
- ap->repeat = settings.value (QString (_buttonActionRepeatKey).arg (button), false ).toBool ();
388
392
}
393
+ ap->repeat (settings.value (QString (_buttonActionRepeatKey).arg (button), false ).toBool ());
389
394
390
395
_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 () ;
394
399
395
400
}
396
401
}
@@ -408,11 +413,14 @@ void Joystick::_saveButtonSettings()
408
413
settings.beginGroup (_name);
409
414
for (int button = 0 ; button < _totalButtonCount; button++) {
410
415
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
+
416
424
}
417
425
}
418
426
}
@@ -570,7 +578,7 @@ void Joystick::run()
570
578
_axisTime.start ();
571
579
for (int buttonIndex = 0 ; buttonIndex < _totalButtonCount; buttonIndex++) {
572
580
if (_buttonActionArray[buttonIndex]) {
573
- _buttonActionArray[buttonIndex]->buttonTime .start ();
581
+ _buttonActionArray[buttonIndex]->buttonTime () .start ();
574
582
}
575
583
}
576
584
while (!_exitThread) {
@@ -619,18 +627,18 @@ void Joystick::_handleButtons()
619
627
for (int buttonIndex = 0 ; buttonIndex < _totalButtonCount; buttonIndex++) {
620
628
if (_rgButtonValues[buttonIndex] == BUTTON_DOWN || _rgButtonValues[buttonIndex] == BUTTON_REPEAT) {
621
629
if (_buttonActionArray[buttonIndex]) {
622
- QString buttonAction = _buttonActionArray[buttonIndex]->action ;
630
+ QString buttonAction = _buttonActionArray[buttonIndex]->action () ;
623
631
if (buttonAction.isEmpty () || buttonAction == _buttonActionNone)
624
632
continue ;
625
- if (!_buttonActionArray[buttonIndex]->repeat ) {
633
+ if (!_buttonActionArray[buttonIndex]->repeat () ) {
626
634
// -- This button just went down
627
635
if (_rgButtonValues[buttonIndex] == BUTTON_DOWN) {
628
636
// Check for a multi-button action
629
637
QList<int > rgButtons = { buttonIndex };
630
638
bool executeButtonAction = true ;
631
639
for (int multiIndex = 0 ; multiIndex < _totalButtonCount; multiIndex++) {
632
640
if (multiIndex != buttonIndex) {
633
- if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) {
641
+ if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action () == buttonAction) {
634
642
// We found a multi-button action
635
643
if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) {
636
644
// So far so good
@@ -652,8 +660,8 @@ void Joystick::_handleButtons()
652
660
} else {
653
661
// -- Process repeat buttons
654
662
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 ();
657
665
qCDebug (JoystickLog) << " Repeat button triggered" << buttonIndex << buttonAction;
658
666
_executeButtonAction (buttonAction, buttonIndex, true );
659
667
}
@@ -666,7 +674,7 @@ void Joystick::_handleButtons()
666
674
if (buttonIndex < 256 ) {
667
675
if (lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) {
668
676
if (_buttonActionArray[buttonIndex]) {
669
- QString buttonAction = _buttonActionArray[buttonIndex]->action ;
677
+ QString buttonAction = _buttonActionArray[buttonIndex]->action () ;
670
678
if (buttonAction.isEmpty () || buttonAction == _buttonActionNone)
671
679
continue ;
672
680
qCDebug (JoystickLog) << " Button up" << buttonIndex << buttonAction;
@@ -879,21 +887,21 @@ void Joystick::setButtonRepeat(int button, bool repeat)
879
887
if (!_validButton (button) || !_buttonActionArray[button]) {
880
888
return ;
881
889
}
882
- _buttonActionArray[button]->repeat = repeat;
883
- _buttonActionArray[button]->buttonTime .start ();
890
+ _buttonActionArray[button]->repeat ( repeat) ;
891
+ _buttonActionArray[button]->buttonTime () .start ();
884
892
// -- Save to settings
885
893
QSettings settings;
886
894
settings.beginGroup (_settingsGroup);
887
895
settings.beginGroup (_name);
888
- settings.setValue (QString (_buttonActionRepeatKey).arg (button), _buttonActionArray[button]->repeat );
896
+ settings.setValue (QString (_buttonActionRepeatKey).arg (button), _buttonActionArray[button]->repeat () );
889
897
}
890
898
891
899
bool Joystick::getButtonRepeat (int button)
892
900
{
893
901
if (!_validButton (button) || !_buttonActionArray[button]) {
894
902
return false ;
895
903
}
896
- return _buttonActionArray[button]->repeat ;
904
+ return _buttonActionArray[button]->repeat () ;
897
905
}
898
906
899
907
void Joystick::setButtonAction (int button, const QString& action)
@@ -907,20 +915,24 @@ void Joystick::setButtonAction(int button, const QString& action)
907
915
settings.beginGroup (_name);
908
916
if (action.isEmpty () || action == _buttonActionNone) {
909
917
if (_buttonActionArray[button]) {
910
- _buttonActionArray[button]->deleteLater ();
911
- _buttonActionArray[button] = nullptr ;
912
918
// -- Clear from settings
913
- // TODO clear if PWM type
914
919
settings.remove (QString (_buttonActionNameKey).arg (button));
915
920
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 ;
916
928
}
917
929
} else {
918
930
bool isPwmAction = assignableActionIsPwm (action);
919
931
AssignedButtonAction *ap;
920
932
921
933
if (!_buttonActionArray[button]) {
922
934
if (isPwmAction) {
923
- ap = new AssignedButtonRcPwmOverrideAction (this , action, 0 , 0 , false );
935
+ ap = new AssignedButtonAction (this , action, 0 , 0 , false );
924
936
} else {
925
937
ap = new AssignedButtonAction (this , action);
926
938
}
@@ -929,23 +941,28 @@ void Joystick::setButtonAction(int button, const QString& action)
929
941
if (isPwmAction) {
930
942
// TODO fix it better, maybe one aciton with PWM values in it
931
943
_buttonActionArray[button]->deleteLater ();
932
- ap = new AssignedButtonRcPwmOverrideAction (this , action, 0 , 0 , false );
944
+ ap = new AssignedButtonAction (this , action, 0 , 0 , false );
933
945
_buttonActionArray[button] = ap;
934
946
}
935
- _buttonActionArray[button]->action = action;
947
+ _buttonActionArray[button]->action ( action) ;
936
948
}
937
949
// -- Make sure repeat is off if this action doesn't support repeats
938
950
int idx = _findAssignableButtonAction (action);
939
951
if (idx >= 0 ) {
940
952
AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[idx]);
941
953
if (!p->canRepeat ()) {
942
- _buttonActionArray[button]->repeat = false ;
954
+ _buttonActionArray[button]->repeat ( false ) ;
943
955
}
944
956
}
945
957
946
958
// -- 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
+ }
949
966
}
950
967
emit buttonActionsChanged ();
951
968
}
@@ -954,7 +971,7 @@ QString Joystick::getButtonAction(int button)
954
971
{
955
972
if (_validButton (button)) {
956
973
if (_buttonActionArray[button]) {
957
- return _buttonActionArray[button]->action ;
974
+ return _buttonActionArray[button]->action () ;
958
975
}
959
976
}
960
977
return QString (_buttonActionNone);
@@ -975,6 +992,7 @@ bool Joystick::assignableActionIsPwm(QString action) {
975
992
}
976
993
977
994
void Joystick::setButtonPwm (int button, bool lowPwm, int value) {
995
+ // TODO czy tutaj przychodzi z GUI i trzeba ustawic tez w _buttonActionArray[button]?
978
996
qDebug (JoystickLog) << " setButtonPwm: " << button << (lowPwm ? " LOW " : " HIGH " ) << value;
979
997
if (_validButton (button)) {
980
998
if (assignableActionIsPwm (button)) {
@@ -994,8 +1012,6 @@ int Joystick::getButtonPwm(int button, bool lowPwm) {
994
1012
qDebug (JoystickLog) << " getButtonPwm: " << button << (lowPwm ? " LOW " : " HIGH " );
995
1013
if (_validButton (button)) {
996
1014
if (assignableActionIsPwm (button)) {
997
- // const AssignedButtonRcPwmOverrideAction *ap = dynamic_cast<const AssignedButtonRcPwmOverrideAction *>(_buttonActionArray[button]);
998
- // int rc = ap->channel();
999
1015
QSettings settings;
1000
1016
settings.beginGroup (_settingsGroup);
1001
1017
settings.beginGroup (_name);
@@ -1016,25 +1032,24 @@ void Joystick::setButtonPwmLatch(int button, bool latch)
1016
1032
}
1017
1033
qCDebug (JoystickLog) << " PWM Latch mode for button " << button << (latch ? " enabled" : " disabled" );
1018
1034
// 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);
1021
1037
1022
1038
// TODO finish saving settings
1023
- /*
1039
+
1024
1040
QSettings settings;
1025
1041
settings.beginGroup (_settingsGroup);
1026
1042
settings.beginGroup (_name);
1027
- settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
1028
- */
1043
+ settings.setValue (QString (_buttonActionLatchPwmValueKey).arg (button), action->pwmLatchMode ());
1029
1044
}
1030
1045
1031
1046
bool Joystick::getButtonPwmLatch (int button) {
1032
1047
if (!_validButton (button) || !_buttonActionArray[button]) {
1033
1048
return false ;
1034
1049
}
1035
1050
// 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 ();
1038
1053
}
1039
1054
1040
1055
QStringList Joystick::buttonActions ()
@@ -1328,6 +1343,7 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
1328
1343
_assignableButtonActions.append (new AssignableButtonAction (this , _buttonActionGripperGrab));
1329
1344
_assignableButtonActions.append (new AssignableButtonAction (this , _buttonActionGripperRelease));
1330
1345
1346
+ // TODO(bzd) take channel nos from config, especially max
1331
1347
for (int ch = 8 ; ch <= 16 ;ch++) {
1332
1348
_assignableButtonActions.append (new AssignableButtonAction (this , QString (" Channel %1 direct PWM" ).arg (ch)));
1333
1349
}
@@ -1347,23 +1363,8 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
1347
1363
bool Joystick::_executeRcOverrideButtonAction (int buttonIndex, bool buttonDown)
1348
1364
{
1349
1365
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 ;
1353
1368
}
1354
1369
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;*/
1369
1370
}
0 commit comments