From 4a4a65ed695e22308c8097f0ceaa43fbe647d17c Mon Sep 17 00:00:00 2001 From: Zdanek Date: Mon, 6 Mar 2023 09:12:06 +0100 Subject: [PATCH 01/11] Fix CMake scripts to run custom builds. Base changes to enable building and running QGC custom builds. QT build script are sufficient to run custom build but CMake scripts totally ignore all custom changes. There's lot of work to be done in matter of resources which I did not address at all in this commit. --- custom-example/custom.cmake | 42 ++++++++ qgroundcontrol.pro | 5 +- src/AnalyzeView/CMakeLists.txt | 14 ++- src/AutoPilotPlugins/CMakeLists.txt | 149 ++++++++++++++++------------ src/FirmwarePlugin/CMakeLists.txt | 66 +++++++++--- src/Settings/CMakeLists.txt | 18 +++- src/Vehicle/Vehicle.cc | 16 +-- src/Vehicle/Vehicle.h | 6 +- src/VideoManager/CMakeLists.txt | 14 ++- src/VideoReceiver/CMakeLists.txt | 9 +- src/comm/TCPLink.cc | 2 +- 11 files changed, 243 insertions(+), 98 deletions(-) create mode 100644 custom-example/custom.cmake diff --git a/custom-example/custom.cmake b/custom-example/custom.cmake new file mode 100644 index 00000000000..53312e68352 --- /dev/null +++ b/custom-example/custom.cmake @@ -0,0 +1,42 @@ +message(STATUS "Adding Custom Plugin") + +# you can disable usb camera +set(QGC_DISABLE_UVC OFF) + +# disable video streaming +set(DISABLE_VIDEOSTREAMING OFF) +# disable video streaming +set(QGC_DISABLE_MAVLINK_INSPECTOR OFF ) + +# disable airmap +set(DISABLE_AIRMAP OFF) + +# disable Ardupilot mavlink dialect +set(QGC_DISABLE_APM_MAVLINK ON) +# disable Ardupilot firmware plugin +set(QGC_DISABLE_APM_PLUGIN ON) +# disable PX4 firmware plugin +# set(QGC_DISABLE_PX4_PLUGIN ON) +# disable Ardupilot firmware plugin factory +set(QGC_DISABLE_APM_PLUGIN_FACTORY ON) +# disable PX4 firmware plugin factory +set(QGC_DISABLE_PX4_PLUGIN_FACTORY ON) + +set(CUSTOMCLASS "CustomPlugin") +set(CUSTOMHEADER \"CustomPlugin.h\") + +set(CUSTOM_SRC_PATH ${CMAKE_CURRENT_SOURCE_DIR}/custom/src) + +include_directories(${CUSTOM_SRC_PATH}) +include_directories(${CUSTOM_SRC_PATH}/FirmwarePlugin) +include_directories(${CUSTOM_SRC_PATH}/AutoPilotPlugin) + +set(CUSTOM_SRC + ${CUSTOM_SRC_PATH}/CustomPlugin.cc + ${CUSTOM_SRC_PATH}/AutoPilotPlugin/CustomAutoPilotPlugin.cc + ${CUSTOM_SRC_PATH}/FirmwarePlugin/CustomFirmwarePlugin.cc + ${CUSTOM_SRC_PATH}/FirmwarePlugin/CustomFirmwarePluginFactory.cc + ) + + +set(CUSTOM_RESOURCES ${CMAKE_CURRENT_SOURCE_DIR}/custom/custom.qrc) \ No newline at end of file diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 27e34fd89c7..1f3e45389d4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -153,7 +153,7 @@ iOSBuild { # disable both the Plugin and PluginFactory entries. To include custom support # for an existing plugin type disable PluginFactory only. Then provide you own # implementation of FirmwarePluginFactory and use the FirmwarePlugin and -# AutoPilotPlugin classes as the base clase for your derived plugin +# AutoPilotPlugin classes as the base class for your derived plugin # implementation. contains (CONFIG, QGC_DISABLE_APM_PLUGIN) { @@ -1376,6 +1376,9 @@ contains (CONFIG, DISABLE_VIDEOSTREAMING) { } !VideoEnabled { +# TODO bzd here propably should go: +# DEFINES -= QGC_GST_STREAMING + INCLUDEPATH += \ src/VideoReceiver diff --git a/src/AnalyzeView/CMakeLists.txt b/src/AnalyzeView/CMakeLists.txt index ad410bb3853..96c165062b1 100644 --- a/src/AnalyzeView/CMakeLists.txt +++ b/src/AnalyzeView/CMakeLists.txt @@ -7,6 +7,15 @@ if(BUILD_TESTING) ) endif() +if (NOT QGC_DISABLE_MAVLINK_INSPECTOR) + set(MAVLINK_INSPECTOR_SRC + MAVLinkInspectorController.cc + MAVLinkInspectorController.h + ) + set(MAVLINK_INSPECTOR_QML + MAVLinkInspectorPage.qml) +endif() + add_library(AnalyzeView ExifParser.cc ExifParser.h @@ -16,8 +25,7 @@ add_library(AnalyzeView LogDownloadController.h MavlinkConsoleController.cc MavlinkConsoleController.h - MAVLinkInspectorController.cc - MAVLinkInspectorController.h + ${MAVLINK_INSPECTOR_SRC} PX4LogParser.cc PX4LogParser.h ULogParser.cc @@ -33,7 +41,7 @@ add_custom_target(AnalyzeViewQml GeoTagPage.qml LogDownloadPage.qml MavlinkConsolePage.qml - MAVLinkInspectorPage.qml + ${MAVLINK_INSPECTOR_QML} VibrationPage.qml ) diff --git a/src/AutoPilotPlugins/CMakeLists.txt b/src/AutoPilotPlugins/CMakeLists.txt index e737b81d8cb..54d40734819 100644 --- a/src/AutoPilotPlugins/CMakeLists.txt +++ b/src/AutoPilotPlugins/CMakeLists.txt @@ -1,30 +1,74 @@ - -add_subdirectory(APM) add_subdirectory(Common) -add_subdirectory(PX4) -add_library(AutoPilotPlugins - APM/APMAirframeComponent.cc - APM/APMAirframeComponentController.cc - APM/APMAutoPilotPlugin.cc - APM/APMCameraComponent.cc - APM/APMCompassCal.cc - APM/APMFlightModesComponent.cc - APM/APMFlightModesComponentController.cc - APM/APMFollowComponent.cc - APM/APMFollowComponentController.cc - APM/APMHeliComponent.cc - APM/APMLightsComponent.cc - APM/APMMotorComponent.cc - APM/APMPowerComponent.cc - APM/APMRadioComponent.cc - APM/APMSafetyComponent.cc - APM/APMSensorsComponent.cc - APM/APMSensorsComponentController.cc - APM/APMSubFrameComponent.cc - APM/APMSubMotorComponentController.cc - APM/APMTuningComponent.cc +if (NOT QGC_DISABLE_PX4_PLUGIN) + add_subdirectory(PX4) + + set(PX4FirmwarePlugin + PX4/ActuatorComponent.cc + PX4/ActuatorComponent.h + PX4/AirframeComponentAirframes.cc + PX4/AirframeComponentAirframes.h + PX4/AirframeComponent.cc + PX4/AirframeComponentController.cc + PX4/AirframeComponentController.h + PX4/AirframeComponent.h + PX4/CameraComponent.cc + PX4/CameraComponent.h + PX4/FlightModesComponent.cc + PX4/FlightModesComponent.h + PX4/PowerComponent.cc + PX4/PowerComponentController.cc + PX4/PowerComponentController.h + PX4/PowerComponent.h + PX4/PX4AirframeLoader.cc + PX4/PX4AirframeLoader.h + PX4/PX4AutoPilotPlugin.cc + PX4/PX4AutoPilotPlugin.h + PX4/PX4FlightBehavior.cc + PX4/PX4FlightBehavior.h + PX4/PX4RadioComponent.cc + PX4/PX4RadioComponent.h + PX4/PX4SimpleFlightModesController.cc + PX4/PX4SimpleFlightModesController.h + PX4/PX4TuningComponent.cc + PX4/PX4TuningComponent.h + PX4/SafetyComponent.cc + PX4/SafetyComponent.h + PX4/SensorsComponent.cc + PX4/SensorsComponentController.cc + PX4/SensorsComponentController.h + PX4/SensorsComponent.h + ) +endif() + +if (NOT QGC_DISABLE_APM_PLUGIN) + add_subdirectory(APM) + set(APMFirmwarePlugin + APM/APMAirframeComponent.cc + APM/APMAirframeComponentController.cc + APM/APMAutoPilotPlugin.cc + APM/APMCameraComponent.cc + APM/APMCompassCal.cc + APM/APMFlightModesComponent.cc + APM/APMFlightModesComponentController.cc + APM/APMFollowComponent.cc + APM/APMFollowComponentController.cc + APM/APMHeliComponent.cc + APM/APMLightsComponent.cc + APM/APMMotorComponent.cc + APM/APMPowerComponent.cc + APM/APMRadioComponent.cc + APM/APMSafetyComponent.cc + APM/APMSensorsComponent.cc + APM/APMSensorsComponentController.cc + APM/APMSubFrameComponent.cc + APM/APMSubMotorComponentController.cc + APM/APMTuningComponent.cc + ) +endif() + +add_library(AutoPilotPlugins Common/ESP8266Component.cc Common/ESP8266ComponentController.cc Common/MotorComponent.cc @@ -34,42 +78,9 @@ add_library(AutoPilotPlugins Generic/GenericAutoPilotPlugin.cc - PX4/ActuatorComponent.cc - PX4/ActuatorComponent.h - PX4/AirframeComponentAirframes.cc - PX4/AirframeComponentAirframes.h - PX4/AirframeComponent.cc - PX4/AirframeComponentController.cc - PX4/AirframeComponentController.h - PX4/AirframeComponent.h - PX4/CameraComponent.cc - PX4/CameraComponent.h - PX4/FlightModesComponent.cc - PX4/FlightModesComponent.h - PX4/PowerComponent.cc - PX4/PowerComponentController.cc - PX4/PowerComponentController.h - PX4/PowerComponent.h - PX4/PX4AirframeLoader.cc - PX4/PX4AirframeLoader.h - PX4/PX4AutoPilotPlugin.cc - PX4/PX4AutoPilotPlugin.h - PX4/PX4FlightBehavior.cc - PX4/PX4FlightBehavior.h - PX4/PX4RadioComponent.cc - PX4/PX4RadioComponent.h - PX4/PX4SimpleFlightModesController.cc - PX4/PX4SimpleFlightModesController.h - PX4/PX4TuningComponent.cc - PX4/PX4TuningComponent.h - PX4/SafetyComponent.cc - PX4/SafetyComponent.h - PX4/SensorsComponent.cc - PX4/SensorsComponentController.cc - PX4/SensorsComponentController.h - PX4/SensorsComponent.h - AutoPilotPlugin.cc + ${APMFirmwarePlugin} + ${PX4FirmwarePlugin} ) target_link_libraries(AutoPilotPlugins @@ -81,7 +92,23 @@ target_include_directories(AutoPilotPlugins INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC - APM Common - PX4 ) + +if (NOT QGC_DISABLE_APM_PLUGIN) + target_include_directories(AutoPilotPlugins + INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC + APM + ) +endif() + +if (NOT QGC_DISABLE_PX4_PLUGIN) + target_include_directories(AutoPilotPlugins + INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC + PX4 + ) +endif() diff --git a/src/FirmwarePlugin/CMakeLists.txt b/src/FirmwarePlugin/CMakeLists.txt index c2f51adae42..5f9a588178d 100644 --- a/src/FirmwarePlugin/CMakeLists.txt +++ b/src/FirmwarePlugin/CMakeLists.txt @@ -1,23 +1,48 @@ add_subdirectory(APM) +if (NOT QGC_DISABLE_PX4_PLUGIN_FACTORY) + set(PX4FirmwarePluginFactory + PX4/PX4FirmwarePluginFactory.cc + ) +endif() + +if (NOT QGC_DISABLE_PX4_PLUGIN) + + set(PX4FirmwarePlugin + PX4/PX4FirmwarePlugin.cc + PX4/PX4ParameterMetaData.cc + PX4/PX4Resources.qrc + ) +endif() + +if (NOT QGC_DISABLE_APM_PLUGIN_FACTORY) + set(APMFirmwarePluginFactory + APM/APMFirmwarePluginFactory.cc + ) +endif() + +if (NOT QGC_DISABLE_APM_PLUGIN) + set(APMFirmwarePlugin + APM/APMFirmwarePlugin.cc + APM/APMParameterMetaData.cc + APM/ArduCopterFirmwarePlugin.cc + APM/ArduPlaneFirmwarePlugin.cc + APM/ArduRoverFirmwarePlugin.cc + APM/ArduSubFirmwarePlugin.cc + APM/APMResources.qrc + ) +endif() + add_library(FirmwarePlugin CameraMetaData.cc FirmwarePlugin.cc FirmwarePluginManager.cc - APM/APMFirmwarePlugin.cc - APM/APMFirmwarePluginFactory.cc - APM/APMParameterMetaData.cc - APM/ArduCopterFirmwarePlugin.cc - APM/ArduPlaneFirmwarePlugin.cc - APM/ArduRoverFirmwarePlugin.cc - APM/ArduSubFirmwarePlugin.cc - APM/APMResources.qrc - - PX4/PX4FirmwarePlugin.cc - PX4/PX4FirmwarePluginFactory.cc - PX4/PX4ParameterMetaData.cc - PX4/PX4Resources.qrc + ${APMFirmwarePlugin} + ${APMFirmwarePluginFactory} + + ${PX4FirmwarePluginFactory} + ${PX4FirmwarePlugin} ) target_link_libraries(FirmwarePlugin @@ -28,6 +53,17 @@ target_link_libraries(FirmwarePlugin target_include_directories(FirmwarePlugin INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} - APM - ) + ) +if (NOT QGC_DISABLE_APM_PLUGIN) + target_include_directories(FirmwarePlugin + INTERFACE + APM) +endif() + +if (NOT QGC_DISABLE_PX4_PLUGIN) + target_include_directories(FirmwarePlugin + INTERFACE + PX4 + ) +endif() diff --git a/src/Settings/CMakeLists.txt b/src/Settings/CMakeLists.txt index c06f941d3ed..acb9d515386 100644 --- a/src/Settings/CMakeLists.txt +++ b/src/Settings/CMakeLists.txt @@ -1,9 +1,15 @@ +if (NOT QGC_DISABLE_APM_MAVLINK) + set(APM_MAVLINK_SR_SETTINGS_SRC + APMMavlinkStreamRateSettings.cc + APMMavlinkStreamRateSettings.h + + ) +endif() add_library(Settings ADSBVehicleManagerSettings.cc ADSBVehicleManagerSettings.h - APMMavlinkStreamRateSettings.cc - APMMavlinkStreamRateSettings.h + ${APM_MAVLINK_SR_SETTINGS_SRC} AppSettings.cc AppSettings.h AutoConnectSettings.cc @@ -37,8 +43,12 @@ add_library(Settings target_link_libraries(Settings PUBLIC qgc - Qt5::Multimedia ) - +if (NOT QGC_DISABLE_UVC) + target_link_libraries(Settings + PUBLIC + Qt5::Multimedia + ) +endif () target_include_directories(Settings PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8ae6b6b9d27..0f4963122fa 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2688,8 +2688,8 @@ double Vehicle::minimumEquivalentAirspeed() return _firmwarePlugin->minimumEquivalentAirspeed(this); } -bool Vehicle::hasGripper() const -{ +bool Vehicle::hasGripper() const +{ return _firmwarePlugin->hasGripper(this); } @@ -3800,7 +3800,7 @@ void Vehicle::_handleRawImuTemp(mavlink_message_t& message) { // This is used by compass calibration emit mavlinkRawImu(message); - + mavlink_raw_imu_t imuRaw; mavlink_msg_raw_imu_decode(&message, &imuRaw); @@ -4053,8 +4053,8 @@ void Vehicle::doSetHome(const QGeoCoordinate& coord) disconnect(_currentDoSetHomeTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_doSetHomeTerrainReceived); _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; } - // Save the coord for using when our terrain data arrives. If there was a pending terrain query paired with an older coordinate it is safe to - // Override now, as we just disconnected the signal that would trigger the command sending + // Save the coord for using when our terrain data arrives. If there was a pending terrain query paired with an older coordinate it is safe to + // Override now, as we just disconnected the signal that would trigger the command sending _doSetHomeCoordinate = coord; // Now setup and trigger the new terrain query _currentDoSetHomeTerrainAtCoordinateQuery = new TerrainAtCoordinateQuery(true /* autoDelet */); @@ -4297,16 +4297,16 @@ void Vehicle::setGripperAction(GRIPPER_ACTIONS gripperAction) void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption) { switch(gripperOption) { - case Gripper_release: + case Gripper_release: setGripperAction(GRIPPER_ACTION_RELEASE); break; - case Gripper_grab: + case Gripper_grab: setGripperAction(GRIPPER_ACTION_GRAB); break; case Invalid_option: qDebug("unknown function"); break; - default: + default: break; } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index a2ebc5a26c9..7ded16fff54 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -533,14 +533,14 @@ class Vehicle : public FactGroup enum GRIPPER_OPTIONS { - Gripper_release = GRIPPER_ACTION_RELEASE, + Gripper_release = GRIPPER_ACTION_RELEASE, Gripper_grab = GRIPPER_ACTION_GRAB, Invalid_option = GRIPPER_ACTIONS_ENUM_END, - }; + }; Q_ENUM(GRIPPER_OPTIONS) void setGripperAction(GRIPPER_ACTIONS gripperAction); - Q_INVOKABLE void sendGripperAction(GRIPPER_OPTIONS gripperOption); + Q_INVOKABLE void sendGripperAction(GRIPPER_OPTIONS gripperOption); bool fixedWing() const; bool multiRotor() const; diff --git a/src/VideoManager/CMakeLists.txt b/src/VideoManager/CMakeLists.txt index 4ffc81714de..4634b2ff2a6 100644 --- a/src/VideoManager/CMakeLists.txt +++ b/src/VideoManager/CMakeLists.txt @@ -10,9 +10,21 @@ add_library(VideoManager target_link_libraries(VideoManager PUBLIC qgc - Qt5::Multimedia Qt5::OpenGL VideoReceiver ) +if (NOT QGC_DISABLE_UVC) + target_link_libraries(VideoManager + PUBLIC + Qt5::Multimedia + ) +endif () + target_include_directories(VideoManager INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +if (QGC_DISABLE_UVC) +# add_compile_definitions(QGC_DISABLE_UVC) + message(STATUS "VideoManager: UVC disabled") + +endif () \ No newline at end of file diff --git a/src/VideoReceiver/CMakeLists.txt b/src/VideoReceiver/CMakeLists.txt index 243ff62d90e..1fcbbdda46b 100644 --- a/src/VideoReceiver/CMakeLists.txt +++ b/src/VideoReceiver/CMakeLists.txt @@ -21,14 +21,21 @@ add_library(VideoReceiver target_link_libraries(VideoReceiver PUBLIC - Qt5::Multimedia Qt5::OpenGL Qt5::Quick ${EXTRA_LIBRARIES} Settings ) +if (NOT QGC_DISABLE_UVC) + target_link_libraries(VideoReceiver + PUBLIC + Qt5::Multimedia + ) +endif () + target_include_directories(VideoReceiver INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) if (GST_FOUND) target_include_directories(VideoReceiver PUBLIC ${GST_INCLUDE_DIRS}) + set(VideoEnabled TRUE PARENT_SCOPE) endif() diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index d932bdaf663..4c9121e30e8 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -50,7 +50,7 @@ void TCPLink::_writeDebugBytes(const QByteArray data) ascii.append(219); } } - qDebug() << "Sent" << size << "bytes to" << _tcpConfig->host() << ":" << _tcpConfig->port() << "data:"; + qDebug() << "Sent" << data.size() << "bytes to" << _tcpConfig->host() << ":" << _tcpConfig->port() << "data:"; qDebug() << bytes; qDebug() << "ASCII:" << ascii; } From 2cc339034be7ace4c76e275ffbd0da7007e45757 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Thu, 11 Aug 2022 13:10:37 +0200 Subject: [PATCH 02/11] Add Saving PWM values for button RC override --- qgroundcontrol.qrc | 2 +- src/Joystick/CMakeLists.txt | 1 + src/Joystick/Joystick.cc | 112 ++++++++++++- src/Joystick/Joystick.h | 14 +- src/Joystick/JoystickRcOverride.cc | 37 +++++ src/Joystick/JoystickRcOverride.h | 26 +++ src/Vehicle/Vehicle.cc | 59 +++++++ src/Vehicle/Vehicle.h | 8 + src/VehicleSetup/JoystickConfigButtons.qml | 177 +++++++++++++++------ 9 files changed, 383 insertions(+), 53 deletions(-) create mode 100644 src/Joystick/JoystickRcOverride.cc create mode 100644 src/Joystick/JoystickRcOverride.h diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index f07d6a356e5..b1b9d3fc644 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -185,7 +185,7 @@ src/PlanView/TerrainStatus.qml src/PlanView/TakeoffItemMapVisual.qml src/QmlControls/ToolStrip.qml - src/QmlControls/ToolStripHoverButton.qml + src/QmlControls/ToolStripHoverButton.qml src/PlanView/TransectStyleComplexItemEditor.qml src/PlanView/TransectStyleComplexItemStats.qml src/PlanView/TransectStyleComplexItemTabBar.qml diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt index 83b1065fdca..16d93eba0de 100644 --- a/src/Joystick/CMakeLists.txt +++ b/src/Joystick/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(Joystick JoystickManager.cc JoystickSDL.cc JoystickMavCommand.cc + JoystickRcOverride.cc ${EXTRA_SRC} ) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 767d2081812..b7c8f56f29a 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -26,6 +26,8 @@ const char* Joystick::_settingsGroup = "Joysticks"; const char* Joystick::_calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration const char* Joystick::_buttonActionNameKey = "ButtonActionName%1"; const char* Joystick::_buttonActionRepeatKey = "ButtonActionRepeat%1"; +const char* Joystick::_buttonActionLowPwmValueKey = "ButtonActionLowPwm%1"; +const char* Joystick::_buttonActionHighPwmValueKey = "ButtonActionHighPwm%1"; const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_negativeThrustSettingsKey = "NegativeThrust"; const char* Joystick::_exponentialSettingsKey = "Exponential"; @@ -126,6 +128,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json"); + JoystickRcOverride rco(8, 1200, 1800, true); + _rcOverrides.append(rco); } void Joystick::stop() @@ -860,6 +864,59 @@ QString Joystick::getButtonAction(int button) return QString(_buttonActionNone); } +bool Joystick::assignableActionIsPWM(int button) { + qDebug(JoystickLog) << "assignableActionIsPWM: " << button; + if (_validButton(button)) { + auto action = getButtonAction(button); + qDebug(JoystickLog) << "action " << action; + return action.contains("PWM"); + } + return false; +} + +void Joystick::setButtonPWM(int button, bool lowPwm, int value) { + qDebug(JoystickLog) << "setButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ") << value; + if (_validButton(button)) { + if (assignableActionIsPWM(button)) { + auto action = getButtonAction(button); + int rc = getRcChannelFromAction(action); + if (rc < 0) { + return; + } + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if (lowPwm) { + settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value); + } else { + settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value); + } + } + } +} + +int Joystick::getButtonPWM(int button, bool lowPwm) { + qDebug(JoystickLog) << "getButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH "); + if (_validButton(button)) { + if (assignableActionIsPWM(button)) { + auto action = getButtonAction(button); + int rc = getRcChannelFromAction(action); + if (rc < 0) { + return -1; + } + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if (lowPwm) { + return settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); + } else { + return settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); + } + } + } + return -1; +} + QStringList Joystick::buttonActions() { QStringList list; @@ -1045,13 +1102,22 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown) emit gripperAction(GRIPPER_ACTION_RELEASE); } } else { - if (buttonDown && _activeVehicle) { - for (auto& item : _customMavCommands) { - if (action == item.name()) { - item.send(_activeVehicle); - return; + qCDebug(JoystickLog) << "Action " << action << " button " << (buttonDown ? "down" : "up"); + if (_activeVehicle) { + bool actionServed = false; + if (action.startsWith("Channel")) { + actionServed = _executeRcOverrideButtonAction(action, buttonDown); + } + if (buttonDown && !actionServed) { + for (auto& item : _customMavCommands) { + if (action == item.name()) { + item.send(_activeVehicle); + return; + } } } + } else { + qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; } } } @@ -1143,6 +1209,11 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); + for (int ch = 8; ch <= 16;ch++) { + _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch))); + } + + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); for (auto& item : _customMavCommands) { _assignableButtonActions.append(new AssignableButtonAction(this, item.name())); } @@ -1153,3 +1224,34 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) } emit assignableActionsChanged(); } + +bool Joystick::_executeRcOverrideButtonAction(const QString &action, bool buttonDown) +{ + QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); + QRegularExpressionMatch match = re.match(action); + if (match.hasMatch()) { + QString channelNoStr = match.captured(1); + qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr; + uint8_t channelNo = channelNoStr.toInt(); + + for (auto& item : _rcOverrides) { + if (channelNo == item.channel()) { + item.send(_activeVehicle, buttonDown); + return true; + } + } + } + return false; +} + +int Joystick::getRcChannelFromAction(QString actionName) { + QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); + QRegularExpressionMatch match = re.match(actionName); + if (match.hasMatch()) { + QString channelNoStr = match.captured(1); + qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr; + uint8_t channelNo = channelNoStr.toInt(); + return channelNo; + } + return -1; +} diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index fcdc5bf8d82..55be9a33135 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -16,10 +16,11 @@ #include #include +#include "JoystickMavCommand.h" +#include "JoystickRcOverride.h" +#include "MultiVehicleManager.h" #include "QGCLoggingCategory.h" #include "Vehicle.h" -#include "MultiVehicleManager.h" -#include "JoystickMavCommand.h" Q_DECLARE_LOGGING_CATEGORY(JoystickLog) Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog) @@ -118,6 +119,10 @@ class Joystick : public QThread Q_INVOKABLE bool getButtonRepeat (int button); Q_INVOKABLE void setButtonAction (int button, const QString& action); Q_INVOKABLE QString getButtonAction (int button); + Q_INVOKABLE bool assignableActionIsPWM (int button); + Q_INVOKABLE void setButtonPWM (int button, bool lowPwm, int value); + Q_INVOKABLE int getButtonPWM (int button, bool lowPwm); + // Property accessors @@ -307,6 +312,7 @@ class Joystick : public QThread MultiVehicleManager* _multiVehicleManager = nullptr; QList _customMavCommands; + QList _rcOverrides; static const float _minAxisFrequencyHz; static const float _maxAxisFrequencyHz; @@ -320,6 +326,8 @@ class Joystick : public QThread static const char* _calibratedSettingsKey; static const char* _buttonActionNameKey; static const char* _buttonActionRepeatKey; + static const char* _buttonActionLowPwmValueKey; + static const char* _buttonActionHighPwmValueKey; static const char* _throttleModeSettingsKey; static const char* _negativeThrustSettingsKey; static const char* _exponentialSettingsKey; @@ -365,4 +373,6 @@ class Joystick : public QThread private slots: void _activeVehicleChanged(Vehicle* activeVehicle); + bool _executeRcOverrideButtonAction(const QString &action, bool buttonDown); + int getRcChannelFromAction(QString actionName); }; diff --git a/src/Joystick/JoystickRcOverride.cc b/src/Joystick/JoystickRcOverride.cc new file mode 100644 index 00000000000..8b8e3bb7fd6 --- /dev/null +++ b/src/Joystick/JoystickRcOverride.cc @@ -0,0 +1,37 @@ +// +// Created by zdanek on 08.07.2022. +// + +#include "JoystickRcOverride.h" + +#include "Vehicle.h" + +JoystickRcOverride::JoystickRcOverride(const uint8_t rcChannel, + const uint16_t loPwmValue, + const uint16_t hiPwmValue, + bool latch) + : rcChannel(rcChannel) + , loPwmValue(loPwmValue) + , hiPwmValue(hiPwmValue) + , latchMode(latch) +{} + +void JoystickRcOverride::send(Vehicle *vehicle, bool buttonDown) +{ + uint16_t pwmValue = buttonDown ? hiPwmValue : loPwmValue; + + if (latchMode) { + if (buttonDown) { + latchButtonDown = !latchButtonDown; + pwmValue = latchButtonDown ? hiPwmValue : loPwmValue; + } else { + return; + } + } + vehicle->rcChannelOverride(rcChannel, pwmValue); +} + +uint8_t JoystickRcOverride::channel() const +{ + return rcChannel; +}; \ No newline at end of file diff --git a/src/Joystick/JoystickRcOverride.h b/src/Joystick/JoystickRcOverride.h new file mode 100644 index 00000000000..df676a5320e --- /dev/null +++ b/src/Joystick/JoystickRcOverride.h @@ -0,0 +1,26 @@ +// +// Created by zdanek on 08.07.2022. +// + +#pragma once + +#include "Vehicle.h" + +class JoystickRcOverride +{ +public: + JoystickRcOverride(const uint8_t rcChannel, + const uint16_t loPwmValue, + const uint16_t hiPwmValue, + bool latch); + void send(Vehicle* vehicle, bool buttonDown); + uint8_t channel() const; + +private: + uint8_t rcChannel; + uint16_t loPwmValue; + uint16_t hiPwmValue; + bool latchMode; + bool latchButtonDown = false; +}; + diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 0f4963122fa..0ab765fa907 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -4310,3 +4310,62 @@ void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption) break; } } + +void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) +{ + const int maxRcChannels = 16; + if (rcChannel > maxRcChannels) { + qCWarning(VehicleLog) << "Unsupported rc channel " << rcChannel << " to override"; + return; + } + if (pwmValue > 2000 || pwmValue < 1000) { + qCWarning(VehicleLog) << "Bad PWM override value " << pwmValue << " for channel " << rcChannel; + return; + } + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + qCDebug(VehicleLog)<< "rcChannelOverride: primary link gone!"; + return; + } + + if (sharedLink->linkConfiguration()->isHighLatency()) { + return; + } + + qCDebug(VehicleLog) << "Sending RC channel " << rcChannel << " PWM override to " << pwmValue; + + uint16_t override_data[18] = {}; + for (int i = 0; i < 18; i++) { + override_data[i] = UINT16_MAX; + } + override_data[rcChannel - 1] = pwmValue; + + mavlink_message_t message; + + mavlink_msg_rc_channels_override_pack_chan( + static_cast(_mavlink->getSystemId()), + static_cast(_mavlink->getComponentId()), + sharedLink->mavlinkChannel(), + &message, + static_cast(_id), + 0, + override_data[0], + override_data[1], + override_data[2], + override_data[3], + override_data[4], + override_data[5], + override_data[6], + override_data[7], + override_data[8], + override_data[9], + override_data[10], + override_data[11], + override_data[12], + override_data[13], + override_data[14], + override_data[15], + override_data[16], + override_data[17]); + sendMessageOnLinkThreadSafe(sharedLink.get(), message); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7ded16fff54..d7b367ad46d 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -485,6 +485,14 @@ class Vehicle : public FactGroup void setJoystickEnabled (bool enabled); void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons); + /// Sends Channel override value + /// @param rcChannel channel number 1-16 + /// @param pwmValue direct value 1000 - 2000 + void rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue); + /// Sends disabling of channel override + /// @param rcChannel channel number 1-16 + void disableChannelOverride(uint8_t rcChannel); + // Property accesors int id() const{ return _id; } int compId() const{ return _compID; } diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index 7fd3cb143ed..dc946a9e89d 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -52,63 +52,150 @@ ColumnLayout { Repeater { id: buttonActionRepeater model: _activeJoystick ? Math.min(_activeJoystick.totalButtonCount, _maxButtons) : [] - Row { - spacing: ScreenTools.defaultFontPixelWidth - property bool pressed - property var currentAssignableAction: _activeJoystick ? _activeJoystick.assignableActions.get(buttonActionCombo.currentIndex) : null - Rectangle { - anchors.verticalCenter: parent.verticalCenter - width: ScreenTools.defaultFontPixelHeight * 1.5 - height: width - border.width: 1 - border.color: qgcPal.text - color: pressed ? qgcPal.buttonHighlight : qgcPal.button - QGCLabel { - anchors.fill: parent - color: pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText - horizontalAlignment: Text.AlignHCenter - verticalAlignment: Text.AlignVCenter - text: modelData + + Column { + spacing: ScreenTools.defaultFontPixelWidth + + Row { + spacing: ScreenTools.defaultFontPixelWidth + property bool pressed + property var currentAssignableAction: _activeJoystick ? _activeJoystick.assignableActions.get(buttonActionCombo.currentIndex) : null + Rectangle { + anchors.verticalCenter: parent.verticalCenter + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + border.width: 1 + border.color: qgcPal.text + color: parent.pressed ? qgcPal.buttonHighlight : qgcPal.button + QGCLabel { + anchors.fill: parent + color: parent.parent.pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + text: modelData + } + } + QGCComboBox { + id: buttonActionCombo + width: ScreenTools.defaultFontPixelWidth * 26 + model: _activeJoystick ? _activeJoystick.assignableActionTitles : [] + sizeToContents: true + + function _findCurrentButtonAction() { + if(_activeJoystick) { + var i = find(_activeJoystick.buttonActions[modelData]) + if(i < 0) i = 0 + currentIndex = i + } + } + + function _isCurrentButtonActionPWM() { + return _activeJoystick ? _activeJoystick.assignableActionIsPWM(modelData) : false + } + + Component.onCompleted: _findCurrentButtonAction() + onModelChanged: _findCurrentButtonAction() + onActivated: _activeJoystick.setButtonAction(modelData, textAt(index)) + } + QGCCheckBox { + id: repeatCheck + text: qsTr("Repeat") + enabled: parent.currentAssignableAction && _activeJoystick.calibrated && parent.currentAssignableAction.canRepeat + onClicked: { + _activeJoystick.setButtonRepeat(modelData, checked) + } + Component.onCompleted: { + if(_activeJoystick) { + checked = _activeJoystick.getButtonRepeat(modelData) + } + } + anchors.verticalCenter: parent.verticalCenter + } + Item { + width: ScreenTools.defaultFontPixelWidth * 2 + height: 1 } } - QGCComboBox { - id: buttonActionCombo - width: ScreenTools.defaultFontPixelWidth * 26 - model: _activeJoystick ? _activeJoystick.assignableActionTitles : [] - sizeToContents: true - function _findCurrentButtonAction() { + Row { + id: pwmSettings + property bool pwmRowVisible: buttonActionCombo._isCurrentButtonActionPWM() + spacing: ScreenTools.defaultFontPixelWidth + visible: pwmRowVisible + + function _setButtonPwm(button, isLow, pwm) { if(_activeJoystick) { - var i = find(_activeJoystick.buttonActions[modelData]) - if(i < 0) i = 0 - currentIndex = i + if (pwm < 1000) { + pwm = 1000; + } + if (pwm > 2000) { + pwm = 2000; + } + _activeJoystick.setButtonPWM(modelData, isLow, pwm) } } - Component.onCompleted: _findCurrentButtonAction() - onModelChanged: _findCurrentButtonAction() - onActivated: _activeJoystick.setButtonAction(modelData, textAt(index)) - } - QGCCheckBox { - id: repeatCheck - text: qsTr("Repeat") - enabled: currentAssignableAction && _activeJoystick.calibrated && currentAssignableAction.canRepeat - onClicked: { - _activeJoystick.setButtonRepeat(modelData, checked) - } - Component.onCompleted: { + function _getButtonPwm(button, isLow) { + var pwm = -1; if(_activeJoystick) { - checked = _activeJoystick.getButtonRepeat(modelData) + pwm = _activeJoystick.getButtonPWM(modelData, isLow) + } + return pwm == -1 ? "" : pwm; + } + + QGCLabel { + id: lowPwmLabel + text: qsTr("Low") + anchors.verticalCenter: parent.verticalCenter + } + QGCTextField { + width: ScreenTools.defaultFontPixelWidth * 10 + implicitHeight: ScreenTools.implicitTextFieldHeight + visible: true + validator: IntValidator { bottom:1000; top: 2000} + + Component.onCompleted: { + if(_activeJoystick) { + text = parent._getButtonPwm(modelData, true) + } + } + onEditingFinished: parent._setButtonPwm(modelData, true, text) + + } + QGCLabel { + id: highPwmLabel + text: qsTr("High") + anchors.verticalCenter: parent.verticalCenter + } + QGCTextField { + width: ScreenTools.defaultFontPixelWidth * 10 + implicitHeight: ScreenTools.implicitTextFieldHeight + visible: true + validator: IntValidator { bottom:1000; top: 2000} + Component.onCompleted: { + if(_activeJoystick) { + text = parent._getButtonPwm(modelData, false) + } + } + onEditingFinished: parent._setButtonPwm(modelData, false, text) + } + QGCCheckBox { + id: latchCheck + text: qsTr("Latch") + anchors.verticalCenter: parent.verticalCenter + + onClicked: { + _activeJoystick.setButtonRepeat(modelData, checked) + } + Component.onCompleted: { + if(_activeJoystick) { + checked = _activeJoystick.getButtonRepeat(modelData) + } } } - anchors.verticalCenter: parent.verticalCenter - } - Item { - width: ScreenTools.defaultFontPixelWidth * 2 - height: 1 } } - } + } } } Column { From 1b7ca68596abc359d8946e1351cc5b8549548506 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Wed, 7 Sep 2022 13:21:43 +0200 Subject: [PATCH 03/11] Working without saving and refresh gui --- CodingStyle.h | 4 +- src/Joystick/CMakeLists.txt | 1 - src/Joystick/Joystick.cc | 254 +++++++++++++++------ src/Joystick/Joystick.h | 45 +++- src/Joystick/JoystickRcOverride.cc | 37 --- src/Joystick/JoystickRcOverride.h | 26 --- src/QGCLoggingCategory.h | 2 +- src/VehicleSetup/JoystickConfigButtons.qml | 20 +- 8 files changed, 232 insertions(+), 157 deletions(-) delete mode 100644 src/Joystick/JoystickRcOverride.cc delete mode 100644 src/Joystick/JoystickRcOverride.h diff --git a/CodingStyle.h b/CodingStyle.h index 2fe6dbb2566..b25ee40d758 100644 --- a/CodingStyle.h +++ b/CodingStyle.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2016 QGROUNDCONTROL PROJECT + * (c) 2009-2022 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -31,7 +31,7 @@ Q_DECLARE_LOGGING_CATEGORY(CodingStyleLog) /// Here is the class documentation. Class names are PascalCase. If you override any of the Qt base classes to provide /// generic base implementations for widespread use prefix the class name with QGC. For example: -/// QGCMessageBox - is a QGC special vesion of Qt MessageBox +/// QGCMessageBox - is a QGC special version of Qt MessageBox /// QGCPalette - is a QGC special version of Qt Palette /// For normal single use classes do no prefix them name with QGC. diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt index 16d93eba0de..83b1065fdca 100644 --- a/src/Joystick/CMakeLists.txt +++ b/src/Joystick/CMakeLists.txt @@ -12,7 +12,6 @@ add_library(Joystick JoystickManager.cc JoystickSDL.cc JoystickMavCommand.cc - JoystickRcOverride.cc ${EXTRA_SRC} ) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index b7c8f56f29a..49868a66c3a 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -28,6 +28,7 @@ const char* Joystick::_buttonActionNameKey = "ButtonActionName%1"; const char* Joystick::_buttonActionRepeatKey = "ButtonActionRepeat%1"; const char* Joystick::_buttonActionLowPwmValueKey = "ButtonActionLowPwm%1"; const char* Joystick::_buttonActionHighPwmValueKey = "ButtonActionHighPwm%1"; +const char* Joystick::_buttonActionLatchPwmValueKey = "ButtonActionLatchPwm%1"; const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_negativeThrustSettingsKey = "NegativeThrust"; const char* Joystick::_exponentialSettingsKey = "Exponential"; @@ -88,12 +89,69 @@ const float Joystick::_maxAxisFrequencyHz = 200.0f; const float Joystick::_minButtonFrequencyHz = 0.25f; const float Joystick::_maxButtonFrequencyHz = 50.0f; -AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString name) +AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action) : QObject(parent) - , action(name) + , action(action) { } +AssignedButtonRcPwmOverrideAction::AssignedButtonRcPwmOverrideAction( + QObject *parent, const QString action, const uint16_t loPwmValue, const uint16_t hiPwmValue, bool latch) + : AssignedButtonAction(parent, action) + , _loPwmValue(loPwmValue) + , _hiPwmValue(hiPwmValue) + , _latchMode(latch) + , _rcChannel(getRcChannelFromAction(action)) +{ +} + +void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown) +{ + uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue; + if (_latchMode) { + qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_latchButtonDown ? "down" : "up"); + + if (buttonDown) { + _latchButtonDown = !_latchButtonDown; + pwmValue = _latchButtonDown ? _hiPwmValue : _loPwmValue; + qCDebug(JoystickLog) << " calculated new PWM Value " << pwmValue; + } else { + qCDebug(JoystickLog) << "since button up - exiting"; + return; + } + } + + vehicle->rcChannelOverride(_rcChannel, pwmValue); +} + +uint8_t AssignedButtonRcPwmOverrideAction::channel() const +{ + return _rcChannel; +}; + +void AssignedButtonRcPwmOverrideAction::setLatchMode(bool latch) +{ + _latchMode = latch; +} + +bool AssignedButtonRcPwmOverrideAction::getLatchMode() +{ + return _latchMode; +} + +uint8_t AssignedButtonRcPwmOverrideAction::getRcChannelFromAction(const QString action) +{ + QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); + QRegularExpressionMatch match = re.match(action); + if (match.hasMatch()) { + QString channelNoStr = match.captured(1); + qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr; + uint8_t channelNo = channelNoStr.toInt(); + return channelNo; + } + return 0; +} + AssignableButtonAction::AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_) : QObject(parent) , _action(action_) @@ -128,8 +186,6 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json"); - JoystickRcOverride rco(8, 1200, 1800, true); - _rcOverrides.append(rco); } void Joystick::stop() @@ -301,27 +357,50 @@ void Joystick::_loadSettings() // FunctionAxis mappings are always stored in TX mode 2 // Remap to stored TX mode in settings _remapAxes(2, _transmitterMode, _rgFunctionAxis); + //TODO add with real channels +// for (int channel = 8; channel <= 16; channel++) { +// _rcOverrides.append(nullptr); +// } for (int button = 0; button < _totalButtonCount; button++) { - QString a = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); - if(!a.isEmpty() && a != _buttonActionNone) { - if(_buttonActionArray[button]) { + QString action = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); + if (!action.isEmpty() && action != _buttonActionNone) { + if (_buttonActionArray[button]) { _buttonActionArray[button]->deleteLater(); } - AssignedButtonAction* ap = new AssignedButtonAction(this, a); - ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + + AssignedButtonAction *ap; + if (assignableActionIsPwm(action)) { + int lowPwm = settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); + int highPwm = settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); + bool latch = settings.value(QString(_buttonActionLatchPwmValueKey).arg(button), false).toBool(); + qCDebug(JoystickLog) << "_loadSettings button:action:pwm" << button << action << "L:" << lowPwm + << " H:" << highPwm << " Latch:" << latch; +// int idx = channel - 8; +// if (_rcOverrides[idx]) { +// delete _rcOverrides[idx]; +// } + ap = new AssignedButtonRcPwmOverrideAction(this, action, lowPwm, highPwm, false); + //.append(JoystickRcOverride(channel, lowPwm, highPwm, latch)); + } else { + ap = new AssignedButtonAction(this, action); + ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + } + _buttonActionArray[button] = ap; _buttonActionArray[button]->buttonTime.start(); - qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action + << _buttonActionArray[button]->repeat; + } } - if (badSettings) { _calibrated = false; settings.setValue(_calibratedSettingsKey, false); } } + void Joystick::_saveButtonSettings() { QSettings settings; @@ -332,6 +411,9 @@ void Joystick::_saveButtonSettings() settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + if (assignableActionIsPwm(button)) { + //TODO finish, but low priority + } } } } @@ -566,7 +648,7 @@ void Joystick::_handleButtons() } if (executeButtonAction) { qCDebug(JoystickLog) << "Action triggered" << rgButtons << buttonAction; - _executeButtonAction(buttonAction, true); + _executeButtonAction(buttonAction, buttonIndex, true); } } } else { @@ -575,7 +657,7 @@ void Joystick::_handleButtons() if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { _buttonActionArray[buttonIndex]->buttonTime.start(); qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction; - _executeButtonAction(buttonAction, true); + _executeButtonAction(buttonAction, buttonIndex, true); } } } @@ -590,7 +672,7 @@ void Joystick::_handleButtons() if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) continue; qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction; - _executeButtonAction(buttonAction, false); + _executeButtonAction(buttonAction, buttonIndex, false); } } } @@ -830,23 +912,39 @@ void Joystick::setButtonAction(int button, const QString& action) _buttonActionArray[button]->deleteLater(); _buttonActionArray[button] = nullptr; //-- Clear from settings + //TODO clear if PWM type settings.remove(QString(_buttonActionNameKey).arg(button)); settings.remove(QString(_buttonActionRepeatKey).arg(button)); } } else { + bool isPwmAction = assignableActionIsPwm(action); + AssignedButtonAction *ap; + if(!_buttonActionArray[button]) { - _buttonActionArray[button] = new AssignedButtonAction(this, action); + if (isPwmAction) { + ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false); + } else { + ap = new AssignedButtonAction(this, action); + } + _buttonActionArray[button] = ap; } else { + if (isPwmAction) { + //TODO fix it better, maybe one aciton with PWM values in it + _buttonActionArray[button]->deleteLater(); + ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false); + _buttonActionArray[button] = ap; + } _buttonActionArray[button]->action = action; - //-- Make sure repeat is off if this action doesn't support repeats - int idx = _findAssignableButtonAction(action); - if(idx >= 0) { - AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); - if(!p->canRepeat()) { - _buttonActionArray[button]->repeat = false; - } + } + //-- Make sure repeat is off if this action doesn't support repeats + int idx = _findAssignableButtonAction(action); + if(idx >= 0) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); + if(!p->canRepeat()) { + _buttonActionArray[button]->repeat = false; } } + //-- Save to settings settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); @@ -864,8 +962,8 @@ QString Joystick::getButtonAction(int button) return QString(_buttonActionNone); } -bool Joystick::assignableActionIsPWM(int button) { - qDebug(JoystickLog) << "assignableActionIsPWM: " << button; +bool Joystick::assignableActionIsPwm(int button) { + qDebug(JoystickLog) << "assignableActionIsPwm: " << button; if (_validButton(button)) { auto action = getButtonAction(button); qDebug(JoystickLog) << "action " << action; @@ -874,15 +972,14 @@ bool Joystick::assignableActionIsPWM(int button) { return false; } -void Joystick::setButtonPWM(int button, bool lowPwm, int value) { - qDebug(JoystickLog) << "setButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ") << value; +bool Joystick::assignableActionIsPwm(QString action) { + return action.contains("PWM"); +} + +void Joystick::setButtonPwm(int button, bool lowPwm, int value) { + qDebug(JoystickLog) << "setButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ") << value; if (_validButton(button)) { - if (assignableActionIsPWM(button)) { - auto action = getButtonAction(button); - int rc = getRcChannelFromAction(action); - if (rc < 0) { - return; - } + if (assignableActionIsPwm(button)) { QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -895,15 +992,12 @@ void Joystick::setButtonPWM(int button, bool lowPwm, int value) { } } -int Joystick::getButtonPWM(int button, bool lowPwm) { - qDebug(JoystickLog) << "getButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH "); +int Joystick::getButtonPwm(int button, bool lowPwm) { + qDebug(JoystickLog) << "getButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH "); if (_validButton(button)) { - if (assignableActionIsPWM(button)) { - auto action = getButtonAction(button); - int rc = getRcChannelFromAction(action); - if (rc < 0) { - return -1; - } + if (assignableActionIsPwm(button)) { +// const AssignedButtonRcPwmOverrideAction *ap = dynamic_cast(_buttonActionArray[button]); +// int rc = ap->channel(); QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -917,6 +1011,34 @@ int Joystick::getButtonPWM(int button, bool lowPwm) { return -1; } +void Joystick::setButtonPwmLatch(int button, bool latch) +{ + if (!_validButton(button) || !_buttonActionArray[button]) { + return; + } + qCDebug(JoystickLog) << "PWM Latch mode for button " << button << (latch ? " enabled" : " disabled"); + //TODO check is really this class + AssignedButtonRcPwmOverrideAction *action = dynamic_cast(_buttonActionArray[button]); + action->setLatchMode(latch); + + //TODO finish saving settings +/* + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + */ +} + +bool Joystick::getButtonPwmLatch(int button) { + if (!_validButton(button) || !_buttonActionArray[button]) { + return false; + } + //TODO check is really this class + AssignedButtonRcPwmOverrideAction *action = dynamic_cast(_buttonActionArray[button]); + return action->getLatchMode(); +} + QStringList Joystick::buttonActions() { QStringList list; @@ -1039,8 +1161,7 @@ void Joystick::setCalibrationMode(bool calibrating) } } - -void Joystick::_executeButtonAction(const QString& action, bool buttonDown) +void Joystick::_executeButtonAction(const QString &action, int buttonIndex, bool buttonDown) { if (!_activeVehicle || !_activeVehicle->joystickEnabled() || action == _buttonActionNone) { return; @@ -1105,8 +1226,8 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown) qCDebug(JoystickLog) << "Action " << action << " button " << (buttonDown ? "down" : "up"); if (_activeVehicle) { bool actionServed = false; - if (action.startsWith("Channel")) { - actionServed = _executeRcOverrideButtonAction(action, buttonDown); + if (assignableActionIsPwm(action)) { + actionServed = _executeRcOverrideButtonAction(buttonIndex, buttonDown); } if (buttonDown && !actionServed) { for (auto& item : _customMavCommands) { @@ -1225,33 +1346,26 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) emit assignableActionsChanged(); } -bool Joystick::_executeRcOverrideButtonAction(const QString &action, bool buttonDown) +bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown) { - QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); - QRegularExpressionMatch match = re.match(action); - if (match.hasMatch()) { - QString channelNoStr = match.captured(1); - qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr; - uint8_t channelNo = channelNoStr.toInt(); - - for (auto& item : _rcOverrides) { - if (channelNo == item.channel()) { - item.send(_activeVehicle, buttonDown); - return true; - } - } + if (_buttonActionArray[buttonIndex]) { + AssignedButtonRcPwmOverrideAction* buttonAction = + dynamic_cast(_buttonActionArray[buttonIndex]); + buttonAction->send(_activeVehicle, buttonDown); } - return false; -} - -int Joystick::getRcChannelFromAction(QString actionName) { - QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); - QRegularExpressionMatch match = re.match(actionName); - if (match.hasMatch()) { - QString channelNoStr = match.captured(1); - qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr; - uint8_t channelNo = channelNoStr.toInt(); - return channelNo; + return false; +/* int channelNo = getRcChannelFromAction(action); + if (channelNo == -1) { + return false; } - return -1; + qCDebug(JoystickLog) << "RC override matched for channel " << channelNo; + //TODO make a map rcChannel : override (ev null if not set) + for (auto& item : _rcOverrides) { + if (channelNo == item->channel()) { + qCDebug(JoystickLog) << "Sending RC override " << channelNo; + item->send(_activeVehicle, buttonDown); + return true; + } + } + return false;*/ } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 55be9a33135..8ec4b4f73d2 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -17,7 +17,6 @@ #include #include "JoystickMavCommand.h" -#include "JoystickRcOverride.h" #include "MultiVehicleManager.h" #include "QGCLoggingCategory.h" #include "Vehicle.h" @@ -30,12 +29,36 @@ Q_DECLARE_METATYPE(GRIPPER_ACTIONS) class AssignedButtonAction : public QObject { Q_OBJECT public: - AssignedButtonAction(QObject* parent, const QString name); + AssignedButtonAction(QObject* parent, const QString action); QString action; QElapsedTimer buttonTime; bool repeat = false; }; +/// Action of RC Pwm value override assigned to button +class AssignedButtonRcPwmOverrideAction : public AssignedButtonAction { + Q_OBJECT +public: + AssignedButtonRcPwmOverrideAction(QObject* parent, const QString name, + const uint16_t loPwmValue, + const uint16_t hiPwmValue, + bool latch); + void send(Vehicle* vehicle, bool buttonDown); + uint8_t channel() const; + void setLatchMode(bool latch); + bool getLatchMode(); + static uint8_t getRcChannelFromAction(const QString action); + +private: + const uint8_t _rcChannel; + uint16_t _loPwmValue; + uint16_t _hiPwmValue; + bool _latchMode; + bool _latchButtonDown = false; + +}; + + /// Assignable Button Action class AssignableButtonAction : public QObject { Q_OBJECT @@ -117,11 +140,14 @@ class Joystick : public QThread Q_INVOKABLE void setButtonRepeat (int button, bool repeat); Q_INVOKABLE bool getButtonRepeat (int button); + Q_INVOKABLE void setButtonPwmLatch (int button, bool latch); + Q_INVOKABLE bool getButtonPwmLatch (int button); Q_INVOKABLE void setButtonAction (int button, const QString& action); Q_INVOKABLE QString getButtonAction (int button); - Q_INVOKABLE bool assignableActionIsPWM (int button); - Q_INVOKABLE void setButtonPWM (int button, bool lowPwm, int value); - Q_INVOKABLE int getButtonPWM (int button, bool lowPwm); + Q_INVOKABLE bool assignableActionIsPwm (int button); + Q_INVOKABLE bool assignableActionIsPwm (QString action); + Q_INVOKABLE void setButtonPwm (int button, bool lowPwm, int value); + Q_INVOKABLE int getButtonPwm (int button, bool lowPwm); // Property accessors @@ -236,7 +262,7 @@ class Joystick : public QThread void _saveButtonSettings (); void _loadSettings (); float _adjustRange (int value, Calibration_t calibration, bool withDeadbands); - void _executeButtonAction (const QString& action, bool buttonDown); + void _executeButtonAction (const QString &action, int buttonIndex, bool buttonDown); int _findAssignableButtonAction(const QString& action); bool _validAxis (int axis) const; bool _validButton (int button) const; @@ -311,8 +337,7 @@ class Joystick : public QThread QStringList _availableActionTitles; MultiVehicleManager* _multiVehicleManager = nullptr; - QList _customMavCommands; - QList _rcOverrides; + QList _customMavCommands; static const float _minAxisFrequencyHz; static const float _maxAxisFrequencyHz; @@ -328,6 +353,7 @@ class Joystick : public QThread static const char* _buttonActionRepeatKey; static const char* _buttonActionLowPwmValueKey; static const char* _buttonActionHighPwmValueKey; + static const char* _buttonActionLatchPwmValueKey; static const char* _throttleModeSettingsKey; static const char* _negativeThrustSettingsKey; static const char* _exponentialSettingsKey; @@ -373,6 +399,5 @@ class Joystick : public QThread private slots: void _activeVehicleChanged(Vehicle* activeVehicle); - bool _executeRcOverrideButtonAction(const QString &action, bool buttonDown); - int getRcChannelFromAction(QString actionName); + bool _executeRcOverrideButtonAction(int buttonIndex, bool buttonDown); }; diff --git a/src/Joystick/JoystickRcOverride.cc b/src/Joystick/JoystickRcOverride.cc deleted file mode 100644 index 8b8e3bb7fd6..00000000000 --- a/src/Joystick/JoystickRcOverride.cc +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by zdanek on 08.07.2022. -// - -#include "JoystickRcOverride.h" - -#include "Vehicle.h" - -JoystickRcOverride::JoystickRcOverride(const uint8_t rcChannel, - const uint16_t loPwmValue, - const uint16_t hiPwmValue, - bool latch) - : rcChannel(rcChannel) - , loPwmValue(loPwmValue) - , hiPwmValue(hiPwmValue) - , latchMode(latch) -{} - -void JoystickRcOverride::send(Vehicle *vehicle, bool buttonDown) -{ - uint16_t pwmValue = buttonDown ? hiPwmValue : loPwmValue; - - if (latchMode) { - if (buttonDown) { - latchButtonDown = !latchButtonDown; - pwmValue = latchButtonDown ? hiPwmValue : loPwmValue; - } else { - return; - } - } - vehicle->rcChannelOverride(rcChannel, pwmValue); -} - -uint8_t JoystickRcOverride::channel() const -{ - return rcChannel; -}; \ No newline at end of file diff --git a/src/Joystick/JoystickRcOverride.h b/src/Joystick/JoystickRcOverride.h deleted file mode 100644 index df676a5320e..00000000000 --- a/src/Joystick/JoystickRcOverride.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by zdanek on 08.07.2022. -// - -#pragma once - -#include "Vehicle.h" - -class JoystickRcOverride -{ -public: - JoystickRcOverride(const uint8_t rcChannel, - const uint16_t loPwmValue, - const uint16_t hiPwmValue, - bool latch); - void send(Vehicle* vehicle, bool buttonDown); - uint8_t channel() const; - -private: - uint8_t rcChannel; - uint16_t loPwmValue; - uint16_t hiPwmValue; - bool latchMode; - bool latchButtonDown = false; -}; - diff --git a/src/QGCLoggingCategory.h b/src/QGCLoggingCategory.h index 7b549118426..50f93b0802e 100644 --- a/src/QGCLoggingCategory.h +++ b/src/QGCLoggingCategory.h @@ -27,7 +27,7 @@ Q_DECLARE_LOGGING_CATEGORY(VideoAllLog) // turns on all individual QGC video log /// @def QGC_LOGGING_CATEGORY /// This is a QGC specific replacement for Q_LOGGING_CATEGORY. It will register the category name into a -/// global list. It's usage is the same as Q_LOGGING_CATEOGRY. +/// global list. It's usage is the same as Q_LOGGING_CATEGORY. #define QGC_LOGGING_CATEGORY(name, ...) \ static QGCLoggingCategory qgcCategory ## name (__VA_ARGS__); \ Q_LOGGING_CATEGORY(name, __VA_ARGS__) diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index dc946a9e89d..afa9b940745 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -55,10 +55,10 @@ ColumnLayout { Column { spacing: ScreenTools.defaultFontPixelWidth + property bool pressed Row { spacing: ScreenTools.defaultFontPixelWidth - property bool pressed property var currentAssignableAction: _activeJoystick ? _activeJoystick.assignableActions.get(buttonActionCombo.currentIndex) : null Rectangle { anchors.verticalCenter: parent.verticalCenter @@ -66,10 +66,10 @@ ColumnLayout { height: width border.width: 1 border.color: qgcPal.text - color: parent.pressed ? qgcPal.buttonHighlight : qgcPal.button + color: parent.parent.pressed ? qgcPal.buttonHighlight : qgcPal.button QGCLabel { anchors.fill: parent - color: parent.parent.pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText + color: parent.parent.parent.pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText horizontalAlignment: Text.AlignHCenter verticalAlignment: Text.AlignVCenter text: modelData @@ -89,8 +89,8 @@ ColumnLayout { } } - function _isCurrentButtonActionPWM() { - return _activeJoystick ? _activeJoystick.assignableActionIsPWM(modelData) : false + function _isCurrentButtonActionPwm() { + return _activeJoystick ? _activeJoystick.assignableActionIsPwm(modelData) : false } Component.onCompleted: _findCurrentButtonAction() @@ -119,7 +119,7 @@ ColumnLayout { Row { id: pwmSettings - property bool pwmRowVisible: buttonActionCombo._isCurrentButtonActionPWM() + property bool pwmRowVisible: buttonActionCombo._isCurrentButtonActionPwm() spacing: ScreenTools.defaultFontPixelWidth visible: pwmRowVisible @@ -131,14 +131,14 @@ ColumnLayout { if (pwm > 2000) { pwm = 2000; } - _activeJoystick.setButtonPWM(modelData, isLow, pwm) + _activeJoystick.setButtonPwm(modelData, isLow, pwm) } } function _getButtonPwm(button, isLow) { var pwm = -1; if(_activeJoystick) { - pwm = _activeJoystick.getButtonPWM(modelData, isLow) + pwm = _activeJoystick.getButtonPwm(modelData, isLow) } return pwm == -1 ? "" : pwm; } @@ -185,11 +185,11 @@ ColumnLayout { anchors.verticalCenter: parent.verticalCenter onClicked: { - _activeJoystick.setButtonRepeat(modelData, checked) + _activeJoystick.setButtonPwmLatch(modelData, checked) } Component.onCompleted: { if(_activeJoystick) { - checked = _activeJoystick.getButtonRepeat(modelData) + checked = _activeJoystick.getButtonPwmLatch(modelData) } } } From b44de62e88d6119e29e2e0514407e95b1bf50270 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Mon, 19 Sep 2022 18:51:40 +0200 Subject: [PATCH 04/11] Refactor PWM override to single joystick action class --- src/Joystick/Joystick.cc | 149 ++++++++++++++++++++------------------- src/Joystick/Joystick.h | 40 ++++++----- src/Vehicle/Vehicle.cc | 1 + 3 files changed, 98 insertions(+), 92 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 49868a66c3a..4e064e73c4d 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -91,13 +91,17 @@ const float Joystick::_maxButtonFrequencyHz = 50.0f; AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action) : QObject(parent) - , action(action) + , _action(action) + , _isPwmOverrideAction(false) + , _rcChannel(0) { } -AssignedButtonRcPwmOverrideAction::AssignedButtonRcPwmOverrideAction( +AssignedButtonAction::AssignedButtonAction( QObject *parent, const QString action, const uint16_t loPwmValue, const uint16_t hiPwmValue, bool latch) - : AssignedButtonAction(parent, action) + : QObject(parent) + , _action(action) + , _isPwmOverrideAction(true) , _loPwmValue(loPwmValue) , _hiPwmValue(hiPwmValue) , _latchMode(latch) @@ -105,8 +109,13 @@ AssignedButtonRcPwmOverrideAction::AssignedButtonRcPwmOverrideAction( { } -void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown) +void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) { + if (!_isPwmOverrideAction) { + qCWarning(JoystickLog) << "send called on non-pwm action"; + return; + } + uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue; if (_latchMode) { qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_latchButtonDown ? "down" : "up"); @@ -124,22 +133,27 @@ void AssignedButtonRcPwmOverrideAction::send(Vehicle *vehicle, bool buttonDown) vehicle->rcChannelOverride(_rcChannel, pwmValue); } -uint8_t AssignedButtonRcPwmOverrideAction::channel() const +uint8_t AssignedButtonAction::pwmChannel() const { return _rcChannel; }; -void AssignedButtonRcPwmOverrideAction::setLatchMode(bool latch) +void AssignedButtonAction::pwmLatchMode(bool latch) { _latchMode = latch; } -bool AssignedButtonRcPwmOverrideAction::getLatchMode() +bool AssignedButtonAction::pwmLatchMode() const { return _latchMode; } -uint8_t AssignedButtonRcPwmOverrideAction::getRcChannelFromAction(const QString action) +bool AssignedButtonAction::isPwmOverrideAction() const +{ + return _isPwmOverrideAction; +} + +uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action) { QRegularExpression re("^Channel (\\d{1,2}) direct PWM"); QRegularExpressionMatch match = re.match(action); @@ -357,10 +371,6 @@ void Joystick::_loadSettings() // FunctionAxis mappings are always stored in TX mode 2 // Remap to stored TX mode in settings _remapAxes(2, _transmitterMode, _rgFunctionAxis); - //TODO add with real channels -// for (int channel = 8; channel <= 16; channel++) { -// _rcOverrides.append(nullptr); -// } for (int button = 0; button < _totalButtonCount; button++) { QString action = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); @@ -376,21 +386,16 @@ void Joystick::_loadSettings() bool latch = settings.value(QString(_buttonActionLatchPwmValueKey).arg(button), false).toBool(); qCDebug(JoystickLog) << "_loadSettings button:action:pwm" << button << action << "L:" << lowPwm << " H:" << highPwm << " Latch:" << latch; -// int idx = channel - 8; -// if (_rcOverrides[idx]) { -// delete _rcOverrides[idx]; -// } - ap = new AssignedButtonRcPwmOverrideAction(this, action, lowPwm, highPwm, false); - //.append(JoystickRcOverride(channel, lowPwm, highPwm, latch)); + ap = new AssignedButtonAction(this, action, lowPwm, highPwm, latch); } else { ap = new AssignedButtonAction(this, action); - ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); } + ap->repeat(settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool()); _buttonActionArray[button] = ap; - _buttonActionArray[button]->buttonTime.start(); - qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action - << _buttonActionArray[button]->repeat; + _buttonActionArray[button]->buttonTime().start(); + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action() + << _buttonActionArray[button]->repeat(); } } @@ -408,11 +413,14 @@ void Joystick::_saveButtonSettings() settings.beginGroup(_name); for (int button = 0; button < _totalButtonCount; button++) { if(_buttonActionArray[button]) { - settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); - qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; - if (assignableActionIsPwm(button)) { - //TODO finish, but low priority + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action()); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat()); + qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action() << _buttonActionArray[button]->repeat(); + if (_buttonActionArray[button]->isPwmOverrideAction()) { + settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm()); + settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm()); + settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); + } } } @@ -570,7 +578,7 @@ void Joystick::run() _axisTime.start(); for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { if(_buttonActionArray[buttonIndex]) { - _buttonActionArray[buttonIndex]->buttonTime.start(); + _buttonActionArray[buttonIndex]->buttonTime().start(); } } while (!_exitThread) { @@ -621,10 +629,10 @@ void Joystick::_handleButtons() for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { if(_rgButtonValues[buttonIndex] == BUTTON_DOWN || _rgButtonValues[buttonIndex] == BUTTON_REPEAT) { if(_buttonActionArray[buttonIndex]) { - QString buttonAction = _buttonActionArray[buttonIndex]->action; + QString buttonAction = _buttonActionArray[buttonIndex]->action(); if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) continue; - if(!_buttonActionArray[buttonIndex]->repeat) { + if(!_buttonActionArray[buttonIndex]->repeat()) { //-- This button just went down if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { // Check for a multi-button action @@ -632,7 +640,7 @@ void Joystick::_handleButtons() bool executeButtonAction = true; for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) { if (multiIndex != buttonIndex) { - if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) { + if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action() == buttonAction) { // We found a multi-button action if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) { // So far so good @@ -654,8 +662,8 @@ void Joystick::_handleButtons() } else { //-- Process repeat buttons int buttonDelay = static_cast(1000.0f / _buttonFrequencyHz); - if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { - _buttonActionArray[buttonIndex]->buttonTime.start(); + if(_buttonActionArray[buttonIndex]->buttonTime().elapsed() > buttonDelay) { + _buttonActionArray[buttonIndex]->buttonTime().start(); qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction; _executeButtonAction(buttonAction, buttonIndex, true); } @@ -668,7 +676,7 @@ void Joystick::_handleButtons() if(buttonIndex < 256) { if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) { if(_buttonActionArray[buttonIndex]) { - QString buttonAction = _buttonActionArray[buttonIndex]->action; + QString buttonAction = _buttonActionArray[buttonIndex]->action(); if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) continue; qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction; @@ -881,13 +889,13 @@ void Joystick::setButtonRepeat(int button, bool repeat) if (!_validButton(button) || !_buttonActionArray[button]) { return; } - _buttonActionArray[button]->repeat = repeat; - _buttonActionArray[button]->buttonTime.start(); + _buttonActionArray[button]->repeat(repeat); + _buttonActionArray[button]->buttonTime().start(); //-- Save to settings QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat()); } bool Joystick::getButtonRepeat(int button) @@ -895,7 +903,7 @@ bool Joystick::getButtonRepeat(int button) if (!_validButton(button) || !_buttonActionArray[button]) { return false; } - return _buttonActionArray[button]->repeat; + return _buttonActionArray[button]->repeat(); } void Joystick::setButtonAction(int button, const QString& action) @@ -909,12 +917,16 @@ void Joystick::setButtonAction(int button, const QString& action) settings.beginGroup(_name); if(action.isEmpty() || action == _buttonActionNone) { if(_buttonActionArray[button]) { - _buttonActionArray[button]->deleteLater(); - _buttonActionArray[button] = nullptr; //-- Clear from settings - //TODO clear if PWM type settings.remove(QString(_buttonActionNameKey).arg(button)); settings.remove(QString(_buttonActionRepeatKey).arg(button)); + if (_buttonActionArray[button]->isPwmOverrideAction()) { + settings.remove(QString(_buttonActionHighPwmValueKey).arg(button)); + settings.remove(QString(_buttonActionLowPwmValueKey).arg(button)); + settings.remove(QString(_buttonActionLatchPwmValueKey).arg(button)); + } + _buttonActionArray[button]->deleteLater(); + _buttonActionArray[button] = nullptr; } } else { bool isPwmAction = assignableActionIsPwm(action); @@ -922,7 +934,7 @@ void Joystick::setButtonAction(int button, const QString& action) if(!_buttonActionArray[button]) { if (isPwmAction) { - ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false); + ap = new AssignedButtonAction(this, action, 0, 0, false); } else { ap = new AssignedButtonAction(this, action); } @@ -931,23 +943,28 @@ void Joystick::setButtonAction(int button, const QString& action) if (isPwmAction) { //TODO fix it better, maybe one aciton with PWM values in it _buttonActionArray[button]->deleteLater(); - ap = new AssignedButtonRcPwmOverrideAction(this, action, 0, 0, false); + ap = new AssignedButtonAction(this, action, 0, 0, false); _buttonActionArray[button] = ap; } - _buttonActionArray[button]->action = action; + _buttonActionArray[button]->action(action); } //-- Make sure repeat is off if this action doesn't support repeats int idx = _findAssignableButtonAction(action); if(idx >= 0) { AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); if(!p->canRepeat()) { - _buttonActionArray[button]->repeat = false; + _buttonActionArray[button]->repeat(false); } } //-- Save to settings - settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action()); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat()); + if (isPwmAction) { + settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm()); + settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm()); + settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); + } } emit buttonActionsChanged(); } @@ -956,7 +973,7 @@ QString Joystick::getButtonAction(int button) { if (_validButton(button)) { if(_buttonActionArray[button]) { - return _buttonActionArray[button]->action; + return _buttonActionArray[button]->action(); } } return QString(_buttonActionNone); @@ -977,6 +994,7 @@ bool Joystick::assignableActionIsPwm(QString action) { } void Joystick::setButtonPwm(int button, bool lowPwm, int value) { + //TODO czy tutaj przychodzi z GUI i trzeba ustawic tez w _buttonActionArray[button]? qDebug(JoystickLog) << "setButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ") << value; if (_validButton(button)) { if (assignableActionIsPwm(button)) { @@ -996,8 +1014,6 @@ int Joystick::getButtonPwm(int button, bool lowPwm) { qDebug(JoystickLog) << "getButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH "); if (_validButton(button)) { if (assignableActionIsPwm(button)) { -// const AssignedButtonRcPwmOverrideAction *ap = dynamic_cast(_buttonActionArray[button]); -// int rc = ap->channel(); QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -1018,16 +1034,15 @@ void Joystick::setButtonPwmLatch(int button, bool latch) } qCDebug(JoystickLog) << "PWM Latch mode for button " << button << (latch ? " enabled" : " disabled"); //TODO check is really this class - AssignedButtonRcPwmOverrideAction *action = dynamic_cast(_buttonActionArray[button]); - action->setLatchMode(latch); + auto *action = (_buttonActionArray[button]); + action->pwmLatchMode(latch); //TODO finish saving settings -/* + QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); - */ + settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), action->pwmLatchMode()); } bool Joystick::getButtonPwmLatch(int button) { @@ -1035,8 +1050,8 @@ bool Joystick::getButtonPwmLatch(int button) { return false; } //TODO check is really this class - AssignedButtonRcPwmOverrideAction *action = dynamic_cast(_buttonActionArray[button]); - return action->getLatchMode(); + auto *action = (_buttonActionArray[button]); + return action->pwmLatchMode(); } QStringList Joystick::buttonActions() @@ -1330,6 +1345,7 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); + // TODO(bzd) take channel nos from config, especially max for (int ch = 8; ch <= 16;ch++) { _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch))); } @@ -1349,23 +1365,8 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown) { if (_buttonActionArray[buttonIndex]) { - AssignedButtonRcPwmOverrideAction* buttonAction = - dynamic_cast(_buttonActionArray[buttonIndex]); - buttonAction->send(_activeVehicle, buttonDown); + _buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown); + return true; } return false; -/* int channelNo = getRcChannelFromAction(action); - if (channelNo == -1) { - return false; - } - qCDebug(JoystickLog) << "RC override matched for channel " << channelNo; - //TODO make a map rcChannel : override (ev null if not set) - for (auto& item : _rcOverrides) { - if (channelNo == item->channel()) { - qCDebug(JoystickLog) << "Sending RC override " << channelNo; - item->send(_activeVehicle, buttonDown); - return true; - } - } - return false;*/ } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 8ec4b4f73d2..5a474a17fe4 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -30,35 +30,39 @@ class AssignedButtonAction : public QObject { Q_OBJECT public: AssignedButtonAction(QObject* parent, const QString action); - QString action; - QElapsedTimer buttonTime; - bool repeat = false; -}; -/// Action of RC Pwm value override assigned to button -class AssignedButtonRcPwmOverrideAction : public AssignedButtonAction { - Q_OBJECT -public: - AssignedButtonRcPwmOverrideAction(QObject* parent, const QString name, - const uint16_t loPwmValue, - const uint16_t hiPwmValue, - bool latch); - void send(Vehicle* vehicle, bool buttonDown); - uint8_t channel() const; - void setLatchMode(bool latch); - bool getLatchMode(); + AssignedButtonAction(QObject* parent, const QString action, + const uint16_t loPwmValue, + const uint16_t hiPwmValue, + bool latch); + QString action() const { return _action; } + void action(const QString& action) { _action = action; } + QElapsedTimer buttonTime() const { return _buttonTime; } + bool repeat() const { return _repeat; } + void repeat(bool repeat) { _repeat = repeat; } + + uint16_t lowPwm() const { return _loPwmValue; } + uint16_t highPwm() const { return _hiPwmValue; } + uint8_t pwmChannel() const; + void pwmLatchMode(bool latch); + bool pwmLatchMode() const; + bool isPwmOverrideAction() const; + void sendPwm(Vehicle* vehicle, bool buttonDown); static uint8_t getRcChannelFromAction(const QString action); private: + QString _action; + QElapsedTimer _buttonTime; + bool _repeat = false; +/// PWM override related fields + const bool _isPwmOverrideAction; const uint8_t _rcChannel; uint16_t _loPwmValue; uint16_t _hiPwmValue; bool _latchMode; bool _latchButtonDown = false; - }; - /// Assignable Button Action class AssignableButtonAction : public QObject { Q_OBJECT diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 0ab765fa907..00e4d691b78 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -4313,6 +4313,7 @@ void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption) void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) { + // TODO(bzd) take from joystick settings const int maxRcChannels = 16; if (rcChannel > maxRcChannels) { qCWarning(VehicleLog) << "Unsupported rc channel " << rcChannel << " to override"; From 702bd06c05fc692c7581a8b84f6e44e8b0c5f3d2 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Wed, 21 Sep 2022 14:00:57 +0200 Subject: [PATCH 05/11] Add GUI react on action dropbox change --- src/Joystick/Joystick.cc | 12 +++++++++++- src/Joystick/Joystick.h | 5 +++++ src/VehicleSetup/JoystickConfigButtons.qml | 7 +------ 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 4e064e73c4d..6189863145e 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -372,7 +372,12 @@ void Joystick::_loadSettings() // Remap to stored TX mode in settings _remapAxes(2, _transmitterMode, _rgFunctionAxis); + while (_pwmVisibilities.length() < _totalButtonCount) { + _pwmVisibilities.push_back(false); + } + for (int button = 0; button < _totalButtonCount; button++) { + _pwmVisibilities[button] = false; QString action = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); if (!action.isEmpty() && action != _buttonActionNone) { if (_buttonActionArray[button]) { @@ -381,6 +386,7 @@ void Joystick::_loadSettings() AssignedButtonAction *ap; if (assignableActionIsPwm(action)) { + _pwmVisibilities[button] = true; int lowPwm = settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); int highPwm = settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); bool latch = settings.value(QString(_buttonActionLatchPwmValueKey).arg(button), false).toBool(); @@ -916,6 +922,7 @@ void Joystick::setButtonAction(int button, const QString& action) settings.beginGroup(_settingsGroup); settings.beginGroup(_name); if(action.isEmpty() || action == _buttonActionNone) { + _pwmVisibilities[button] = false; if(_buttonActionArray[button]) { //-- Clear from settings settings.remove(QString(_buttonActionNameKey).arg(button)); @@ -930,8 +937,10 @@ void Joystick::setButtonAction(int button, const QString& action) } } else { bool isPwmAction = assignableActionIsPwm(action); + _pwmVisibilities[button] = isPwmAction; + qCDebug(JoystickLog) << "setButtonAction: isPwmAction" << isPwmAction; AssignedButtonAction *ap; - +//TODO(bzd) rethink this section if(!_buttonActionArray[button]) { if (isPwmAction) { ap = new AssignedButtonAction(this, action, 0, 0, false); @@ -966,6 +975,7 @@ void Joystick::setButtonAction(int button, const QString& action) settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); } } + emit pwmVisibilitiesChanged(); emit buttonActionsChanged(); } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 5a474a17fe4..085556e6790 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -129,6 +129,7 @@ class Joystick : public QThread Q_PROPERTY(QmlObjectListModel* assignableActions READ assignableActions NOTIFY assignableActionsChanged) Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged) Q_PROPERTY(QString disabledActionName READ disabledActionName CONSTANT) + Q_PROPERTY(QList pwmVisibilities READ pwmVisibilities NOTIFY pwmVisibilitiesChanged) Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) Q_PROPERTY(float axisFrequencyHz READ axisFrequencyHz WRITE setAxisFrequency NOTIFY axisFrequencyHzChanged) @@ -164,6 +165,7 @@ class Joystick : public QThread QmlObjectListModel* assignableActions () { return &_assignableButtonActions; } QStringList assignableActionTitles () { return _availableActionTitles; } QString disabledActionName () { return _buttonActionNone; } + QList pwmVisibilities () { return _pwmVisibilities; } /// Start the polling thread which will in turn emit joystick signals void startPolling(Vehicle* vehicle); @@ -226,6 +228,7 @@ class Joystick : public QThread void calibratedChanged (bool calibrated); void buttonActionsChanged (); void assignableActionsChanged (); + void pwmVisibilitiesChanged (); void throttleModeChanged (int mode); void negativeThrustChanged (bool allowNegative); void exponentialChanged (float exponential); @@ -339,6 +342,8 @@ class Joystick : public QThread QmlObjectListModel _assignableButtonActions; QList _buttonActionArray; QStringList _availableActionTitles; + //TODO(bzd) refactor name + QList _pwmVisibilities; MultiVehicleManager* _multiVehicleManager = nullptr; QList _customMavCommands; diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index afa9b940745..124f40e54e4 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -89,10 +89,6 @@ ColumnLayout { } } - function _isCurrentButtonActionPwm() { - return _activeJoystick ? _activeJoystick.assignableActionIsPwm(modelData) : false - } - Component.onCompleted: _findCurrentButtonAction() onModelChanged: _findCurrentButtonAction() onActivated: _activeJoystick.setButtonAction(modelData, textAt(index)) @@ -119,9 +115,8 @@ ColumnLayout { Row { id: pwmSettings - property bool pwmRowVisible: buttonActionCombo._isCurrentButtonActionPwm() spacing: ScreenTools.defaultFontPixelWidth - visible: pwmRowVisible + visible: _activeJoystick ? _activeJoystick.pwmVisibilities[modelData] : false function _setButtonPwm(button, isLow, pwm) { if(_activeJoystick) { From a1baf1580245b26d75b6a5db793570ef4483f991 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Thu, 22 Sep 2022 16:17:33 +0200 Subject: [PATCH 06/11] Refactor for clearness --- src/Joystick/CMakeLists.txt | 1 - src/Joystick/Joystick.cc | 210 ++++++++++----------- src/Joystick/Joystick.h | 35 ++-- src/VehicleSetup/JoystickConfigButtons.qml | 1 + 4 files changed, 123 insertions(+), 124 deletions(-) diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt index 83b1065fdca..84f2f9a47b1 100644 --- a/src/Joystick/CMakeLists.txt +++ b/src/Joystick/CMakeLists.txt @@ -1,4 +1,3 @@ - set(EXTRA_SRC) if (ANDROID) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 6189863145e..c62e99cf66e 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -93,7 +93,7 @@ AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action : QObject(parent) , _action(action) , _isPwmOverrideAction(false) - , _rcChannel(0) + , _pwmRcChannel(0) { } @@ -104,8 +104,8 @@ AssignedButtonAction::AssignedButtonAction( , _isPwmOverrideAction(true) , _loPwmValue(loPwmValue) , _hiPwmValue(hiPwmValue) - , _latchMode(latch) - , _rcChannel(getRcChannelFromAction(action)) + , _pwmLatchMode(latch) + , _pwmRcChannel(getRcChannelFromAction(action)) { } @@ -117,12 +117,12 @@ void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) } uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue; - if (_latchMode) { - qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_latchButtonDown ? "down" : "up"); + if (_pwmLatchMode) { + qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_pwmLatchButtonDown ? "down" : "up"); if (buttonDown) { - _latchButtonDown = !_latchButtonDown; - pwmValue = _latchButtonDown ? _hiPwmValue : _loPwmValue; + _pwmLatchButtonDown = !_pwmLatchButtonDown; + pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue; qCDebug(JoystickLog) << " calculated new PWM Value " << pwmValue; } else { qCDebug(JoystickLog) << "since button up - exiting"; @@ -130,27 +130,7 @@ void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) } } - vehicle->rcChannelOverride(_rcChannel, pwmValue); -} - -uint8_t AssignedButtonAction::pwmChannel() const -{ - return _rcChannel; -}; - -void AssignedButtonAction::pwmLatchMode(bool latch) -{ - _latchMode = latch; -} - -bool AssignedButtonAction::pwmLatchMode() const -{ - return _latchMode; -} - -bool AssignedButtonAction::isPwmOverrideAction() const -{ - return _isPwmOverrideAction; + vehicle->rcChannelOverride(_pwmRcChannel, pwmValue); } uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action) @@ -182,8 +162,11 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _totalButtonCount(_buttonCount+_hatButtonCount) , _multiVehicleManager(multiVehicleManager) { +<<<<<<< HEAD qRegisterMetaType(); +======= +>>>>>>> Refactor for clearness _rgAxisValues = new int[static_cast(_axisCount)]; _rgCalibration = new Calibration_t[static_cast(_axisCount)]; _rgButtonValues = new uint8_t[static_cast(_totalButtonCount)]; @@ -372,12 +355,13 @@ void Joystick::_loadSettings() // Remap to stored TX mode in settings _remapAxes(2, _transmitterMode, _rgFunctionAxis); - while (_pwmVisibilities.length() < _totalButtonCount) { - _pwmVisibilities.push_back(false); + // Preparing pwm sections visibility map + while (_pwmSettingsVisibilities.length() < _totalButtonCount) { + _pwmSettingsVisibilities.push_back(false); } for (int button = 0; button < _totalButtonCount; button++) { - _pwmVisibilities[button] = false; + _pwmSettingsVisibilities[button] = false; QString action = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); if (!action.isEmpty() && action != _buttonActionNone) { if (_buttonActionArray[button]) { @@ -386,7 +370,7 @@ void Joystick::_loadSettings() AssignedButtonAction *ap; if (assignableActionIsPwm(action)) { - _pwmVisibilities[button] = true; + _pwmSettingsVisibilities[button] = true; int lowPwm = settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); int highPwm = settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); bool latch = settings.value(QString(_buttonActionLatchPwmValueKey).arg(button), false).toBool(); @@ -396,13 +380,14 @@ void Joystick::_loadSettings() } else { ap = new AssignedButtonAction(this, action); } - ap->repeat(settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool()); - _buttonActionArray[button] = ap; _buttonActionArray[button]->buttonTime().start(); + + bool savedRepeatState = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + setButtonRepeatIfAvailable(button, savedRepeatState); + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action() << _buttonActionArray[button]->repeat(); - } } if (badSettings) { @@ -918,67 +903,87 @@ void Joystick::setButtonAction(int button, const QString& action) return; } qCWarning(JoystickLog) << "setButtonAction:" << button << action; - QSettings settings; - settings.beginGroup(_settingsGroup); - settings.beginGroup(_name); - if(action.isEmpty() || action == _buttonActionNone) { - _pwmVisibilities[button] = false; - if(_buttonActionArray[button]) { - //-- Clear from settings - settings.remove(QString(_buttonActionNameKey).arg(button)); - settings.remove(QString(_buttonActionRepeatKey).arg(button)); - if (_buttonActionArray[button]->isPwmOverrideAction()) { - settings.remove(QString(_buttonActionHighPwmValueKey).arg(button)); - settings.remove(QString(_buttonActionLowPwmValueKey).arg(button)); - settings.remove(QString(_buttonActionLatchPwmValueKey).arg(button)); - } + + if (action.isEmpty() || action == _buttonActionNone) { + _pwmSettingsVisibilities[button] = false; + if (_buttonActionArray[button]) { + removeButtonSettings(button); _buttonActionArray[button]->deleteLater(); _buttonActionArray[button] = nullptr; } } else { bool isPwmAction = assignableActionIsPwm(action); - _pwmVisibilities[button] = isPwmAction; - qCDebug(JoystickLog) << "setButtonAction: isPwmAction" << isPwmAction; - AssignedButtonAction *ap; -//TODO(bzd) rethink this section - if(!_buttonActionArray[button]) { - if (isPwmAction) { - ap = new AssignedButtonAction(this, action, 0, 0, false); - } else { - ap = new AssignedButtonAction(this, action); - } - _buttonActionArray[button] = ap; + _pwmSettingsVisibilities[button] = isPwmAction; + qCDebug(JoystickLog) << "setButtonAction: isPwmAction " << isPwmAction; + + if (!_buttonActionArray[button]) { + _buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1000, 2000, false) + : new AssignedButtonAction(this, action); } else { if (isPwmAction) { - //TODO fix it better, maybe one aciton with PWM values in it + // PWM Actions are not reusable _buttonActionArray[button]->deleteLater(); - ap = new AssignedButtonAction(this, action, 0, 0, false); - _buttonActionArray[button] = ap; - } - _buttonActionArray[button]->action(action); - } - //-- Make sure repeat is off if this action doesn't support repeats - int idx = _findAssignableButtonAction(action); - if(idx >= 0) { - AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); - if(!p->canRepeat()) { - _buttonActionArray[button]->repeat(false); + _buttonActionArray[button] = new AssignedButtonAction(this, action, 0, 0, false); + } else { + if (_buttonActionArray[button]->isPwmOverrideAction()) { + _buttonActionArray[button]->deleteLater(); + _buttonActionArray[button] = new AssignedButtonAction(this, action); + } else { + _buttonActionArray[button]->action(action); + } } } - //-- Save to settings - settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action()); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat()); - if (isPwmAction) { - settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm()); - settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm()); - settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); - } + //-- Make sure repeat is off if this action doesn't support repeats + setButtonRepeatIfAvailable(button, false); + saveButtonSettings(button); } emit pwmVisibilitiesChanged(); emit buttonActionsChanged(); } +void Joystick::setButtonRepeatIfAvailable(int button, bool repeat) +{ + int idx = _findAssignableButtonAction(_buttonActionArray[button]->action()); + if (idx >= 0) { + AssignableButtonAction *p = qobject_cast(_assignableButtonActions[idx]); + if (p->canRepeat()) { + _buttonActionArray[button]->repeat(repeat); + return; + } + } + _buttonActionArray[button]->repeat(false); +} + +void Joystick::saveButtonSettings(int button) +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + //-- Save to settings + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action()); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat()); + if (_buttonActionArray[button]->isPwmOverrideAction()) { + settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm()); + settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm()); + settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); + } +} + +void Joystick::removeButtonSettings(int button) +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + settings.remove(QString(_buttonActionNameKey).arg(button)); + settings.remove(QString(_buttonActionRepeatKey).arg(button)); + if (_buttonActionArray[button]->isPwmOverrideAction()) { + settings.remove(QString(_buttonActionHighPwmValueKey).arg(button)); + settings.remove(QString(_buttonActionLowPwmValueKey).arg(button)); + settings.remove(QString(_buttonActionLatchPwmValueKey).arg(button)); + } +} + QString Joystick::getButtonAction(int button) { if (_validButton(button)) { @@ -989,14 +994,8 @@ QString Joystick::getButtonAction(int button) return QString(_buttonActionNone); } -bool Joystick::assignableActionIsPwm(int button) { - qDebug(JoystickLog) << "assignableActionIsPwm: " << button; - if (_validButton(button)) { - auto action = getButtonAction(button); - qDebug(JoystickLog) << "action " << action; - return action.contains("PWM"); - } - return false; +bool Joystick::assignableButtonActionIsPwm(int button) { + return (_validButton(button) && _buttonActionArray[button]) ? _buttonActionArray[button]->isPwmOverrideAction() : false; } bool Joystick::assignableActionIsPwm(QString action) { @@ -1004,26 +1003,25 @@ bool Joystick::assignableActionIsPwm(QString action) { } void Joystick::setButtonPwm(int button, bool lowPwm, int value) { - //TODO czy tutaj przychodzi z GUI i trzeba ustawic tez w _buttonActionArray[button]? qDebug(JoystickLog) << "setButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ") << value; - if (_validButton(button)) { - if (assignableActionIsPwm(button)) { - QSettings settings; - settings.beginGroup(_settingsGroup); - settings.beginGroup(_name); - if (lowPwm) { - settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value); - } else { - settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value); - } + if (assignableButtonActionIsPwm(button)) { + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if (lowPwm) { + _buttonActionArray[button]->lowPwm(value); + settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value); + } else { + _buttonActionArray[button]->highPwm(value); + settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value); } } + } int Joystick::getButtonPwm(int button, bool lowPwm) { - qDebug(JoystickLog) << "getButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH "); if (_validButton(button)) { - if (assignableActionIsPwm(button)) { + if (assignableButtonActionIsPwm(button)) { QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -1043,12 +1041,9 @@ void Joystick::setButtonPwmLatch(int button, bool latch) return; } qCDebug(JoystickLog) << "PWM Latch mode for button " << button << (latch ? " enabled" : " disabled"); - //TODO check is really this class - auto *action = (_buttonActionArray[button]); + auto action = _buttonActionArray[button]; action->pwmLatchMode(latch); - //TODO finish saving settings - QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -1059,9 +1054,8 @@ bool Joystick::getButtonPwmLatch(int button) { if (!_validButton(button) || !_buttonActionArray[button]) { return false; } - //TODO check is really this class - auto *action = (_buttonActionArray[button]); - return action->pwmLatchMode(); + auto action = (_buttonActionArray[button]); + return _buttonActionArray[button]->isPwmOverrideAction() ? action->pwmLatchMode() : false; } QStringList Joystick::buttonActions() diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 085556e6790..3c6ee4359c5 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -28,6 +28,7 @@ Q_DECLARE_METATYPE(GRIPPER_ACTIONS) /// Action assigned to button class AssignedButtonAction : public QObject { Q_OBJECT + public: AssignedButtonAction(QObject* parent, const QString action); @@ -43,10 +44,11 @@ class AssignedButtonAction : public QObject { uint16_t lowPwm() const { return _loPwmValue; } uint16_t highPwm() const { return _hiPwmValue; } - uint8_t pwmChannel() const; - void pwmLatchMode(bool latch); - bool pwmLatchMode() const; - bool isPwmOverrideAction() const; + void lowPwm(uint16_t value) { _loPwmValue = value; } + void highPwm(uint16_t value) { _hiPwmValue = value; } + void pwmLatchMode(bool latch) { _pwmLatchMode = latch; } + bool pwmLatchMode() const { return _pwmLatchMode; } + bool isPwmOverrideAction() const { return _isPwmOverrideAction; } void sendPwm(Vehicle* vehicle, bool buttonDown); static uint8_t getRcChannelFromAction(const QString action); @@ -56,11 +58,11 @@ class AssignedButtonAction : public QObject { bool _repeat = false; /// PWM override related fields const bool _isPwmOverrideAction; - const uint8_t _rcChannel; uint16_t _loPwmValue; uint16_t _hiPwmValue; - bool _latchMode; - bool _latchButtonDown = false; + bool _pwmLatchMode; + const uint8_t _pwmRcChannel; + bool _pwmLatchButtonDown = false; }; /// Assignable Button Action @@ -149,14 +151,12 @@ class Joystick : public QThread Q_INVOKABLE bool getButtonPwmLatch (int button); Q_INVOKABLE void setButtonAction (int button, const QString& action); Q_INVOKABLE QString getButtonAction (int button); - Q_INVOKABLE bool assignableActionIsPwm (int button); + Q_INVOKABLE bool assignableButtonActionIsPwm(int button); Q_INVOKABLE bool assignableActionIsPwm (QString action); - Q_INVOKABLE void setButtonPwm (int button, bool lowPwm, int value); - Q_INVOKABLE int getButtonPwm (int button, bool lowPwm); - + Q_INVOKABLE void setButtonPwm (int button, bool lowPwm, int value); + Q_INVOKABLE int getButtonPwm (int button, bool lowPwm); // Property accessors - QString name () { return _name; } int totalButtonCount () const{ return _totalButtonCount; } int axisCount () const{ return _axisCount; } @@ -165,7 +165,7 @@ class Joystick : public QThread QmlObjectListModel* assignableActions () { return &_assignableButtonActions; } QStringList assignableActionTitles () { return _availableActionTitles; } QString disabledActionName () { return _buttonActionNone; } - QList pwmVisibilities () { return _pwmVisibilities; } + QList pwmVisibilities () { return _pwmSettingsVisibilities; } /// Start the polling thread which will in turn emit joystick signals void startPolling(Vehicle* vehicle); @@ -295,6 +295,12 @@ class Joystick : public QThread int _mapFunctionMode(int mode, int function); void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); + void removeButtonSettings(int button); + void saveButtonSettings(int button); + /// if repeat is available for action under button, sets passed repeat flag + /// otherwise sets false as safe value + void setButtonRepeatIfAvailable(int button, bool repeat); + // Override from QThread virtual void run(); @@ -342,8 +348,7 @@ class Joystick : public QThread QmlObjectListModel _assignableButtonActions; QList _buttonActionArray; QStringList _availableActionTitles; - //TODO(bzd) refactor name - QList _pwmVisibilities; + QList _pwmSettingsVisibilities; MultiVehicleManager* _multiVehicleManager = nullptr; QList _customMavCommands; diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index 124f40e54e4..db5a757aa27 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -167,6 +167,7 @@ ColumnLayout { implicitHeight: ScreenTools.implicitTextFieldHeight visible: true validator: IntValidator { bottom:1000; top: 2000} + Component.onCompleted: { if(_activeJoystick) { text = parent._getButtonPwm(modelData, false) From 7afaa5e28f3fcccdce2a8718ef91952ab7f4da28 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Wed, 23 Nov 2022 16:42:24 +0100 Subject: [PATCH 07/11] Add reseting PWM field values on button select combo change --- src/Joystick/Joystick.cc | 4 +-- src/Vehicle/Vehicle.cc | 7 +++-- src/VehicleSetup/JoystickConfigButtons.qml | 31 ++++++++++++++++++++++ 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index c62e99cf66e..1f34b965bc9 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -123,13 +123,13 @@ void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) if (buttonDown) { _pwmLatchButtonDown = !_pwmLatchButtonDown; pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue; - qCDebug(JoystickLog) << " calculated new PWM Value " << pwmValue; } else { - qCDebug(JoystickLog) << "since button up - exiting"; + qCDebug(JoystickLog) << "since button is up - exiting"; return; } } + qCDebug(JoystickLog) << " Sending PWM Value " << pwmValue; vehicle->rcChannelOverride(_pwmRcChannel, pwmValue); } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 00e4d691b78..718b25f0c14 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2274,7 +2274,8 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool send void Vehicle::_sendMessageMultipleNext() { if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) { - qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; +//bzd to be enabled but it spams too much + //qCDebug(VehicleLog) << "_sendMessageMultipleNext: " << _nextSendMessageMultipleIndex << ", mesg id: " << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); if (sharedLink) { @@ -4340,7 +4341,9 @@ void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) override_data[i] = UINT16_MAX; } override_data[rcChannel - 1] = pwmValue; - + for (int i = 0; i < 18; i++) { + qCDebug(VehicleLog) << "Override data " << i << " is " << override_data[i]; + } mavlink_message_t message; mavlink_msg_rc_channels_override_pack_chan( diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index db5a757aa27..4900a5d260e 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -144,11 +144,26 @@ ColumnLayout { anchors.verticalCenter: parent.verticalCenter } QGCTextField { + id: lowPwmValue width: ScreenTools.defaultFontPixelWidth * 10 implicitHeight: ScreenTools.implicitTextFieldHeight visible: true validator: IntValidator { bottom:1000; top: 2000} + Connections { + target: buttonActionCombo + onCurrentIndexChanged: { + if(_activeJoystick) { + console.log("index changed, ", buttonActionCombo.currentIndex) + console.log("index changed, ", modelData) + console.log("index changed, ", target) + var pwm = pwmSettings._getButtonPwm(modelData, true) + console.log("pwm ", pwm) + lowPwmValue.text = pwm; + } + } + } + Component.onCompleted: { if(_activeJoystick) { text = parent._getButtonPwm(modelData, true) @@ -163,11 +178,27 @@ ColumnLayout { anchors.verticalCenter: parent.verticalCenter } QGCTextField { + id: highPwmValue width: ScreenTools.defaultFontPixelWidth * 10 implicitHeight: ScreenTools.implicitTextFieldHeight visible: true validator: IntValidator { bottom:1000; top: 2000} + Connections { + target: buttonActionCombo + function onCurrentIndexChanged(index) { + if(_activeJoystick) { + console.log("index changed, ", buttonActionCombo.currentIndex) + console.log("index changed, ", modelData) + console.log("index changed, ", target) + console.log("text ", target.text) + var pwm = pwmSettings._getButtonPwm(modelData, false) + console.log("pwm ", pwm) + highPwmValue.text = pwm; + } + } + } + Component.onCompleted: { if(_activeJoystick) { text = parent._getButtonPwm(modelData, false) From b02cd510de62d62812c00604ef44e4a9388feecd Mon Sep 17 00:00:00 2001 From: Zdanek Date: Wed, 7 Dec 2022 09:19:03 +0100 Subject: [PATCH 08/11] Add periodic RC override sending. Fix PWM input fields update --- src/AnalyzeView/MAVLinkInspectorController.h | 2 + src/AnalyzeView/MAVLinkInspectorPage.qml | 6 ++ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 6 ++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 3 +- src/FirmwarePlugin/FirmwarePlugin.cc | 5 ++ src/FirmwarePlugin/FirmwarePlugin.h | 4 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 5 ++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/Joystick/Joystick.cc | 89 ++++++++++++++------ src/Joystick/Joystick.h | 35 +++++--- src/Vehicle/Vehicle.cc | 30 +++++-- src/Vehicle/Vehicle.h | 2 + src/comm/MAVLinkProtocol.cc | 16 +++- src/comm/MAVLinkProtocol.h | 1 + 14 files changed, 155 insertions(+), 50 deletions(-) diff --git a/src/AnalyzeView/MAVLinkInspectorController.h b/src/AnalyzeView/MAVLinkInspectorController.h index 83736d842eb..dd6c251e8ef 100644 --- a/src/AnalyzeView/MAVLinkInspectorController.h +++ b/src/AnalyzeView/MAVLinkInspectorController.h @@ -95,6 +95,7 @@ class QGCMAVLinkMessage : public QObject { Q_PROPERTY(QString name READ name CONSTANT) Q_PROPERTY(qreal messageHz READ messageHz NOTIFY freqChanged) Q_PROPERTY(quint64 count READ count NOTIFY countChanged) + Q_PROPERTY(int mavlinkVer READ mavlinkVer CONSTANT) Q_PROPERTY(QmlObjectListModel* fields READ fields CONSTANT) Q_PROPERTY(bool fieldSelected READ fieldSelected NOTIFY fieldSelectedChanged) Q_PROPERTY(bool selected READ selected NOTIFY selectedChanged) @@ -107,6 +108,7 @@ class QGCMAVLinkMessage : public QObject { QString name () { return _name; } qreal messageHz () const{ return _messageHz; } quint64 count () const{ return _count; } + int mavlinkVer () const{ return _message.magic == 0xFE ? 1 : 2; } quint64 lastCount () const{ return _lastCount; } QmlObjectListModel* fields () { return &_fields; } bool fieldSelected () const{ return _fieldSelected; } diff --git a/src/AnalyzeView/MAVLinkInspectorPage.qml b/src/AnalyzeView/MAVLinkInspectorPage.qml index a0b0ee03efb..e5ad1bae9c9 100644 --- a/src/AnalyzeView/MAVLinkInspectorPage.qml +++ b/src/AnalyzeView/MAVLinkInspectorPage.qml @@ -158,6 +158,12 @@ AnalyzePage { QGCLabel { text: curMessage ? curMessage.count : "" } + QGCLabel { + text: qsTr("Mavlink ver:") + } + QGCLabel { + text: curMessage ? curMessage.mavlinkVer : "" + } } Item { height: ScreenTools.defaultFontPixelHeight; width: 1 } //--------------------------------------------------------- diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index d404c70b7f9..c3a9683cbfb 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -260,6 +260,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* /*vehicle*/, mavlink_ // and lower the severity on them so that they don't pop in the users face. QString messageText = _getMessageText(message); + qDebug(APMFirmwarePluginLog) << "StatusText: " << messageText; if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) { _adjustCalibrationMessageSeverity(message); return true; @@ -1073,3 +1074,8 @@ QMutex& APMFirmwarePlugin::_reencodeMavlinkChannelMutex() static QMutex _mutex{}; return _mutex; } + +bool APMFirmwarePlugin::supportsRcChannelOverride(void) +{ + return true; +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index f4587f94137..c7ae3734720 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -77,9 +77,10 @@ class APMFirmwarePlugin : public FirmwarePlugin QObject* _loadParameterMetaData (const QString& metaDataFile) override; QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } - QString getHobbsMeter (Vehicle* vehicle) override; + QString getHobbsMeter (Vehicle* vehicle) override; bool hasGripper (const Vehicle* vehicle) const override; const QVariantList& toolIndicators (const Vehicle* vehicle) override; + bool supportsRcChannelOverride(void) override; protected: /// All access to singleton is through stack specific implementation diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 574f2618f9a..80cd2add873 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -154,6 +154,11 @@ bool FirmwarePlugin::supportsJSButton(void) return false; } +bool FirmwarePlugin::supportsRcChannelOverride(void) +{ + return false; +} + bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { Q_UNUSED(vehicle); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a3956747263..4d2a2be1122 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -211,6 +211,10 @@ class FirmwarePlugin : public QObject /// to be assigned via parameters in firmware. Default is false. virtual bool supportsJSButton(void); + /// Returns true if the firmware supports the mavlink RC_CHANNELS_OVERRIDE message + /// Default is false. + virtual bool supportsRcChannelOverride(void); + /// Returns true if the firmware supports calibrating motor interference offsets for the compass /// (CompassMot). Default is true. virtual bool supportsMotorInterference(void); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 1d75579c5c9..b0b17801b56 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -736,4 +736,9 @@ bool PX4FirmwarePlugin::hasGripper(const Vehicle* vehicle) const return _hasGripper; } return false; +} + +bool PX4FirmwarePlugin::supportsRcChannelOverride(void) +{ + return false; } \ No newline at end of file diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index ff8e9eb8c02..6f261ff8578 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -73,6 +73,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin bool supportsNegativeThrust (Vehicle* vehicle) override; QString getHobbsMeter (Vehicle* vehicle) override; bool hasGripper (const Vehicle* vehicle) const override; + bool supportsRcChannelOverride (void) override; protected: typedef struct { diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 1f34b965bc9..8e8e02e7969 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -10,7 +10,6 @@ #include "Joystick.h" #include "QGC.h" -#include "AutoPilotPlugin.h" #include "UAS.h" #include "QGCApplication.h" #include "VideoManager.h" @@ -88,6 +87,7 @@ const float Joystick::_minAxisFrequencyHz = 0.25f; const float Joystick::_maxAxisFrequencyHz = 200.0f; const float Joystick::_minButtonFrequencyHz = 0.25f; const float Joystick::_maxButtonFrequencyHz = 50.0f; +const float Joystick::_rcOverrideFrequencyHz = 0.5f; AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString action) : QObject(parent) @@ -109,11 +109,11 @@ AssignedButtonAction::AssignedButtonAction( { } -void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) +int16_t AssignedButtonAction::calculatePwm(Vehicle *vehicle, bool buttonDown) { if (!_isPwmOverrideAction) { qCWarning(JoystickLog) << "send called on non-pwm action"; - return; + return -1; } uint16_t pwmValue = buttonDown ? _hiPwmValue : _loPwmValue; @@ -121,16 +121,16 @@ void AssignedButtonAction::sendPwm(Vehicle *vehicle, bool buttonDown) qCDebug(JoystickLog) << "Latch mode, current saved button state " << (_pwmLatchButtonDown ? "down" : "up"); if (buttonDown) { + // altering latch state _pwmLatchButtonDown = !_pwmLatchButtonDown; - pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue; - } else { - qCDebug(JoystickLog) << "since button is up - exiting"; - return; } + pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue; } - qCDebug(JoystickLog) << " Sending PWM Value " << pwmValue; - vehicle->rcChannelOverride(_pwmRcChannel, pwmValue); + qCDebug(JoystickLog) << " Calculated PWM Value " << pwmValue << " for action " << _action; + //vehicle->rcChannelOverride(_pwmRcChannel, pwmValue); + + return pwmValue; } uint8_t AssignedButtonAction::getRcChannelFromAction(const QString action) @@ -162,11 +162,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _totalButtonCount(_buttonCount+_hatButtonCount) , _multiVehicleManager(multiVehicleManager) { -<<<<<<< HEAD qRegisterMetaType(); -======= ->>>>>>> Refactor for clearness _rgAxisValues = new int[static_cast(_axisCount)]; _rgCalibration = new Calibration_t[static_cast(_axisCount)]; _rgButtonValues = new uint8_t[static_cast(_totalButtonCount)]; @@ -177,6 +174,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC _rgButtonValues[i] = BUTTON_UP; _buttonActionArray.append(nullptr); } + _buildActionList(_multiVehicleManager->activeVehicle()); _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); _loadSettings(); @@ -277,6 +275,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); setTXMode(mode); } + _clearRcOverrideButtonActions(); } void Joystick::_loadSettings() @@ -384,7 +383,7 @@ void Joystick::_loadSettings() _buttonActionArray[button]->buttonTime().start(); bool savedRepeatState = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); - setButtonRepeatIfAvailable(button, savedRepeatState); + _setButtonRepeatIfAvailable(button, savedRepeatState); qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action() << _buttonActionArray[button]->repeat(); @@ -411,7 +410,6 @@ void Joystick::_saveButtonSettings() settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), _buttonActionArray[button]->lowPwm()); settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), _buttonActionArray[button]->highPwm()); settings.setValue(QString(_buttonActionLatchPwmValueKey).arg(button), _buttonActionArray[button]->pwmLatchMode()); - } } } @@ -567,6 +565,7 @@ void Joystick::run() _open(); //-- Reset timers _axisTime.start(); + _rcOverrideTimer.start(); for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { if(_buttonActionArray[buttonIndex]) { _buttonActionArray[buttonIndex]->buttonTime().start(); @@ -576,6 +575,7 @@ void Joystick::run() _update(); _handleButtons(); _handleAxis(); + _handleRcOverride(); QGC::SLEEP::msleep(qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2); } _close(); @@ -775,6 +775,26 @@ void Joystick::_handleAxis() } } +void Joystick::_handleRcOverride() +{ + if (!_activeVehicle || !_activeVehicle->supportsRcChannelOverride()) { + return; + } + const int rcOverrideInterval = static_cast(1000.0f / _rcOverrideFrequencyHz); + + if (_rcOverrideActive || _rcOverrideTimer.elapsed() > rcOverrideInterval) { + _rcOverrideActive = false; + _rcOverrideTimer.start(); + uint16_t overrideData[18] = {UINT16_MAX}; + overrideData[16] = _mapRcOverrideToRelease(17, 0); + overrideData[17] = _mapRcOverrideToRelease(18, 0); + for (int i = 0; i < MAX_RC_CHANNELS; i++) { + overrideData[i] = _rcOverride[i] == -1 ? _mapRcOverrideToRelease(i + 1, 0) : _rcOverride[i]; + } + _activeVehicle->rcChannelsOverride(overrideData); + } +} + void Joystick::startPolling(Vehicle* vehicle) { if (vehicle) { @@ -907,7 +927,7 @@ void Joystick::setButtonAction(int button, const QString& action) if (action.isEmpty() || action == _buttonActionNone) { _pwmSettingsVisibilities[button] = false; if (_buttonActionArray[button]) { - removeButtonSettings(button); + _removeButtonSettings(button); _buttonActionArray[button]->deleteLater(); _buttonActionArray[button] = nullptr; } @@ -917,7 +937,7 @@ void Joystick::setButtonAction(int button, const QString& action) qCDebug(JoystickLog) << "setButtonAction: isPwmAction " << isPwmAction; if (!_buttonActionArray[button]) { - _buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1000, 2000, false) + _buttonActionArray[button] = isPwmAction ? new AssignedButtonAction(this, action, 1500, 1500, false) : new AssignedButtonAction(this, action); } else { if (isPwmAction) { @@ -935,14 +955,14 @@ void Joystick::setButtonAction(int button, const QString& action) } //-- Make sure repeat is off if this action doesn't support repeats - setButtonRepeatIfAvailable(button, false); - saveButtonSettings(button); + _setButtonRepeatIfAvailable(button, false); + _saveButtonSettings(button); } emit pwmVisibilitiesChanged(); emit buttonActionsChanged(); } -void Joystick::setButtonRepeatIfAvailable(int button, bool repeat) +void Joystick::_setButtonRepeatIfAvailable(int button, bool repeat) { int idx = _findAssignableButtonAction(_buttonActionArray[button]->action()); if (idx >= 0) { @@ -955,7 +975,7 @@ void Joystick::setButtonRepeatIfAvailable(int button, bool repeat) _buttonActionArray[button]->repeat(false); } -void Joystick::saveButtonSettings(int button) +void Joystick::_saveButtonSettings(int button) { QSettings settings; settings.beginGroup(_settingsGroup); @@ -970,7 +990,7 @@ void Joystick::saveButtonSettings(int button) } } -void Joystick::removeButtonSettings(int button) +void Joystick::_removeButtonSettings(int button) { QSettings settings; settings.beginGroup(_settingsGroup); @@ -1349,10 +1369,10 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); - // TODO(bzd) take channel nos from config, especially max - for (int ch = 8; ch <= 16;ch++) { - _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch))); + for (int ch = 8; ch <= MAX_RC_CHANNELS; ch++) { + _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch))); } + _clearRcOverrideButtonActions(); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); for (auto& item : _customMavCommands) { @@ -1369,8 +1389,27 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown) { if (_buttonActionArray[buttonIndex]) { - _buttonActionArray[buttonIndex]->sendPwm(_activeVehicle, buttonDown); + uint8_t channel = _buttonActionArray[buttonIndex]->rcChannel(); + auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(_activeVehicle, buttonDown); + _rcOverride[channel-1] = pwm; + _rcOverrideActive = true; return true; } return false; } + +void Joystick::_clearRcOverrideButtonActions() { + qCDebug(JoystickLog) << "Clearing RC override button actions"; + for (int i = 0; i < MAX_RC_CHANNELS; i++) { + _rcOverride[i] = -1; + } +} + +uint16_t Joystick::_mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value) { + // UINT16_MAX - ignore + // 0 Release + if (value == 0) { + return rcChannel < 9 ? 0 : UINT16_MAX - 1; + } + return value; +} \ No newline at end of file diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 3c6ee4359c5..be8aee2212a 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -25,6 +25,8 @@ Q_DECLARE_LOGGING_CATEGORY(JoystickLog) Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog) Q_DECLARE_METATYPE(GRIPPER_ACTIONS) +static const int MAX_RC_CHANNELS = 16; + /// Action assigned to button class AssignedButtonAction : public QObject { Q_OBJECT @@ -49,10 +51,12 @@ class AssignedButtonAction : public QObject { void pwmLatchMode(bool latch) { _pwmLatchMode = latch; } bool pwmLatchMode() const { return _pwmLatchMode; } bool isPwmOverrideAction() const { return _isPwmOverrideAction; } - void sendPwm(Vehicle* vehicle, bool buttonDown); - static uint8_t getRcChannelFromAction(const QString action); + int16_t calculatePwm(Vehicle* vehicle, bool buttonDown); + uint8_t rcChannel() const { return _pwmRcChannel; } private: + static uint8_t getRcChannelFromAction(const QString action); + QString _action; QElapsedTimer _buttonTime; bool _repeat = false; @@ -151,8 +155,8 @@ class Joystick : public QThread Q_INVOKABLE bool getButtonPwmLatch (int button); Q_INVOKABLE void setButtonAction (int button, const QString& action); Q_INVOKABLE QString getButtonAction (int button); - Q_INVOKABLE bool assignableButtonActionIsPwm(int button); - Q_INVOKABLE bool assignableActionIsPwm (QString action); + Q_INVOKABLE bool assignableButtonActionIsPwm (int button); + Q_INVOKABLE bool assignableActionIsPwm (QString action); Q_INVOKABLE void setButtonPwm (int button, bool lowPwm, int value); Q_INVOKABLE int getButtonPwm (int button, bool lowPwm); @@ -179,12 +183,6 @@ class Joystick : public QThread void stop(); -/* - // Joystick index used by sdl library - // Settable because sdl library remaps indices after certain events - virtual int index(void) = 0; - virtual void setIndex(int index) = 0; -*/ virtual bool requiresCalibration(void) { return true; } int throttleMode (); @@ -275,6 +273,7 @@ class Joystick : public QThread bool _validButton (int button) const; void _handleAxis (); void _handleButtons (); + void _handleRcOverride (); void _buildActionList (Vehicle* activeVehicle); void _pitchStep (int direction); @@ -295,11 +294,15 @@ class Joystick : public QThread int _mapFunctionMode(int mode, int function); void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); - void removeButtonSettings(int button); - void saveButtonSettings(int button); + //TODO(bzd) change private members to use _ prefix + void _removeButtonSettings(int button); + void _saveButtonSettings(int button); /// if repeat is available for action under button, sets passed repeat flag /// otherwise sets false as safe value - void setButtonRepeatIfAvailable(int button, bool repeat); + void _setButtonRepeatIfAvailable(int button, bool repeat); + bool _executeRcOverrideButtonAction(int buttonIndex, bool buttonDown); + void _clearRcOverrideButtonActions(); + uint16_t _mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value); // Override from QThread virtual void run(); @@ -330,6 +333,7 @@ class Joystick : public QThread float _axisFrequencyHz = _defaultAxisFrequencyHz; float _buttonFrequencyHz = _defaultButtonFrequencyHz; Vehicle* _activeVehicle = nullptr; + int16_t _rcOverride[MAX_RC_CHANNELS]; bool _pollingStartedForCalibration = false; @@ -344,6 +348,8 @@ class Joystick : public QThread static int _transmitterMode; int _rgFunctionAxis[maxFunction] = {}; QElapsedTimer _axisTime; + QElapsedTimer _rcOverrideTimer; + bool _rcOverrideActive = false; QmlObjectListModel _assignableButtonActions; QList _buttonActionArray; @@ -357,6 +363,7 @@ class Joystick : public QThread static const float _maxAxisFrequencyHz; static const float _minButtonFrequencyHz; static const float _maxButtonFrequencyHz; + static const float _rcOverrideFrequencyHz; private: static const char* _rgFunctionSettingsKey[maxFunction]; @@ -413,5 +420,5 @@ class Joystick : public QThread private slots: void _activeVehicleChanged(Vehicle* activeVehicle); - bool _executeRcOverrideButtonAction(int buttonIndex, bool buttonDown); + }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 718b25f0c14..7c73061b02a 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2534,6 +2534,11 @@ bool Vehicle::supportsTerrainFrame() const return !px4Firmware(); } +bool Vehicle::supportsRcChannelOverride() const +{ + return _firmwarePlugin->supportsRcChannelOverride(); +} + QString Vehicle::vehicleTypeName() const { static QMap typeNames = { { MAV_TYPE_GENERIC, tr("Generic micro air vehicle" )}, @@ -4314,7 +4319,7 @@ void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption) void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) { - // TODO(bzd) take from joystick settings + // TODO(bzd) take from joystick settings or from RC settings const int maxRcChannels = 16; if (rcChannel > maxRcChannels) { qCWarning(VehicleLog) << "Unsupported rc channel " << rcChannel << " to override"; @@ -4324,6 +4329,19 @@ void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) qCWarning(VehicleLog) << "Bad PWM override value " << pwmValue << " for channel " << rcChannel; return; } + + qCDebug(VehicleLog) << "Sending RC channel " << rcChannel << " PWM override to " << pwmValue; + + uint16_t override_data[18] = {}; + for (int i = 0; i < 18; i++) { + override_data[i] = UINT16_MAX; + } + override_data[rcChannel - 1] = pwmValue; + + rcChannelsOverride(override_data); +} + +void Vehicle::rcChannelsOverride(uint16_t *override_data) { SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); if (!sharedLink) { qCDebug(VehicleLog)<< "rcChannelOverride: primary link gone!"; @@ -4334,16 +4352,12 @@ void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue) return; } - qCDebug(VehicleLog) << "Sending RC channel " << rcChannel << " PWM override to " << pwmValue; - - uint16_t override_data[18] = {}; - for (int i = 0; i < 18; i++) { - override_data[i] = UINT16_MAX; - } - override_data[rcChannel - 1] = pwmValue; for (int i = 0; i < 18; i++) { qCDebug(VehicleLog) << "Override data " << i << " is " << override_data[i]; + if (override_data[i] > UINT16_MAX - 2 && override_data[i] != 0) { + } } + mavlink_message_t message; mavlink_msg_rc_channels_override_pack_chan( diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index d7b367ad46d..b4122facbe2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -489,6 +489,7 @@ class Vehicle : public FactGroup /// @param rcChannel channel number 1-16 /// @param pwmValue direct value 1000 - 2000 void rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue); + void rcChannelsOverride(uint16_t* rcChannels); /// Sends disabling of channel override /// @param rcChannel channel number 1-16 void disableChannelOverride(uint8_t rcChannel); @@ -562,6 +563,7 @@ class Vehicle : public FactGroup bool supportsJSButton () const; bool supportsMotorInterference () const; bool supportsTerrainFrame () const; + bool supportsRcChannelOverride () const; void setGuidedMode(bool guidedMode); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 93f17ba8703..e3220d712b4 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -208,14 +208,23 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) for (int position = 0; position < b.size(); position++) { if (mavlink_parse_char(mavlinkChannel, static_cast(b[position]), &_message, &_status)) { // Got a valid message - if (!link->decodedFirstMavlinkPacket()) { + if (!link->decodedFirstMavlinkPacket() && !_messageIsFromAutopilot(_message)) { + qCritical(MAVLinkProtocolLog) << "First message from GCS is not from autopilot!!!" + << " component " << _message.compid << " system " + << _message.sysid << " msgid " << _message.msgid; + } + if (!link->decodedFirstMavlinkPacket() && _messageIsFromAutopilot(_message)) { link->setDecodedFirstMavlinkPacket(true); + qCDebug(MAVLinkProtocolLog) << "proto " << mavlink_get_proto_version(mavlinkChannel) + << " component " << _message.compid << " system " << _message.sysid << " msgid " << _message.msgid; mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); - if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { + if (true || (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1))) { qCDebug(MAVLinkProtocolLog) << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags; mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; // Set all links to v2 setVersion(200); + } else { + qCDebug(MAVLinkProtocolLog) << "Outbound connection is mavlink 1.0 due to incoming mavlink 1.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags; } } @@ -536,3 +545,6 @@ void MAVLinkProtocol::deleteTempLogFiles(void) } } +bool MAVLinkProtocol::_messageIsFromAutopilot(mavlink_message_t& message) { + return message.compid == MAV_COMP_ID_AUTOPILOT1; +} \ No newline at end of file diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 65d333eaefe..709d9a7c42c 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -159,6 +159,7 @@ private slots: bool _closeLogFile(void); void _startLogging(void); void _stopLogging(void); + bool _messageIsFromAutopilot(mavlink_message_t& message); bool _logSuspendError; ///< true: Logging suspended due to error bool _logSuspendReplay; ///< true: Logging suspended due to replay From 49431b36995c41c95a6af2e599c1540a518f5d59 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Fri, 9 Dec 2022 12:37:01 +0100 Subject: [PATCH 09/11] Mix PWM buttons into single channel --- src/Joystick/Joystick.cc | 15 ++++++++------- src/Joystick/Joystick.h | 2 +- src/Vehicle/Vehicle.cc | 8 +++++--- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 8e8e02e7969..93a1f01541b 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -109,7 +109,7 @@ AssignedButtonAction::AssignedButtonAction( { } -int16_t AssignedButtonAction::calculatePwm(Vehicle *vehicle, bool buttonDown) +int16_t AssignedButtonAction::calculatePwm(bool buttonDown) { if (!_isPwmOverrideAction) { qCWarning(JoystickLog) << "send called on non-pwm action"; @@ -126,10 +126,7 @@ int16_t AssignedButtonAction::calculatePwm(Vehicle *vehicle, bool buttonDown) } pwmValue = _pwmLatchButtonDown ? _hiPwmValue : _loPwmValue; } - qCDebug(JoystickLog) << " Calculated PWM Value " << pwmValue << " for action " << _action; - //vehicle->rcChannelOverride(_pwmRcChannel, pwmValue); - return pwmValue; } @@ -626,12 +623,16 @@ void Joystick::_handleButtons() if(!_buttonActionArray[buttonIndex]->repeat()) { //-- This button just went down if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { - // Check for a multi-button action + // Check for a multi-button action except for PWM actions. + // PWM buttons can produce different PWM values on same channel. + // Therefore low value must be the same for all buttons at the same channel. QList rgButtons = { buttonIndex }; bool executeButtonAction = true; for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) { if (multiIndex != buttonIndex) { - if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action() == buttonAction) { + if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action() == buttonAction + && !_buttonActionArray[multiIndex]->isPwmOverrideAction()) { + qCDebug(JoystickLog) << "Multi-button action:" << buttonAction; // We found a multi-button action if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) { // So far so good @@ -1390,7 +1391,7 @@ bool Joystick::_executeRcOverrideButtonAction(int buttonIndex, bool buttonDown) { if (_buttonActionArray[buttonIndex]) { uint8_t channel = _buttonActionArray[buttonIndex]->rcChannel(); - auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(_activeVehicle, buttonDown); + auto pwm = _buttonActionArray[buttonIndex]->calculatePwm(buttonDown); _rcOverride[channel-1] = pwm; _rcOverrideActive = true; return true; diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index be8aee2212a..ecd433983c1 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -51,7 +51,7 @@ class AssignedButtonAction : public QObject { void pwmLatchMode(bool latch) { _pwmLatchMode = latch; } bool pwmLatchMode() const { return _pwmLatchMode; } bool isPwmOverrideAction() const { return _isPwmOverrideAction; } - int16_t calculatePwm(Vehicle* vehicle, bool buttonDown); + int16_t calculatePwm(bool buttonDown); uint8_t rcChannel() const { return _pwmRcChannel; } private: diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7c73061b02a..f8b44dbf8c4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -4352,9 +4352,11 @@ void Vehicle::rcChannelsOverride(uint16_t *override_data) { return; } - for (int i = 0; i < 18; i++) { - qCDebug(VehicleLog) << "Override data " << i << " is " << override_data[i]; - if (override_data[i] > UINT16_MAX - 2 && override_data[i] != 0) { + if (VehicleLog().isDebugEnabled()) { + for (int i = 0; i < 18; i++) { + if (override_data[i] > 0 && override_data[i] < UINT16_MAX - 1) { + qCDebug(VehicleLog) << "RC Override ch " << i << " => " << override_data[i]; + } } } From 30dafd54e49145974b9fdefc8fecd14035dbc4f6 Mon Sep 17 00:00:00 2001 From: Zdanek Date: Fri, 24 Feb 2023 13:54:19 +0100 Subject: [PATCH 10/11] Multibutton channels --- src/Joystick/Joystick.cc | 49 ++++++++++++++++------ src/Joystick/Joystick.h | 15 ++++++- src/VehicleSetup/JoystickConfigButtons.qml | 31 ++++++++++---- 3 files changed, 71 insertions(+), 24 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 93a1f01541b..d4d446bc13d 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -998,7 +998,7 @@ void Joystick::_removeButtonSettings(int button) settings.beginGroup(_name); settings.remove(QString(_buttonActionNameKey).arg(button)); settings.remove(QString(_buttonActionRepeatKey).arg(button)); - if (_buttonActionArray[button]->isPwmOverrideAction()) { + if (assignableButtonActionIsPwm(button)) { settings.remove(QString(_buttonActionHighPwmValueKey).arg(button)); settings.remove(QString(_buttonActionLowPwmValueKey).arg(button)); settings.remove(QString(_buttonActionLatchPwmValueKey).arg(button)); @@ -1016,43 +1016,53 @@ QString Joystick::getButtonAction(int button) } bool Joystick::assignableButtonActionIsPwm(int button) { - return (_validButton(button) && _buttonActionArray[button]) ? _buttonActionArray[button]->isPwmOverrideAction() : false; + return _validButton(button) && _buttonActionArray[button] && _buttonActionArray[button]->isPwmOverrideAction(); } bool Joystick::assignableActionIsPwm(QString action) { return action.contains("PWM"); } -void Joystick::setButtonPwm(int button, bool lowPwm, int value) { +int Joystick::setButtonPwm(int button, bool lowPwm, int value) { qDebug(JoystickLog) << "setButtonPwm: " << button << (lowPwm ? "LOW " : "HIGH ") << value; if (assignableButtonActionIsPwm(button)) { QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); if (lowPwm) { + /// finds first other button with same action and sets low value to same value, emits error + int anyOtherButtonWithSameAction = _getOtherMultiButtonPWMOverrideButtonIndex(button); + if (anyOtherButtonWithSameAction != -1) { + if (value != _buttonActionArray[anyOtherButtonWithSameAction]->lowPwm()) { + value = _buttonActionArray[anyOtherButtonWithSameAction]->lowPwm(); + qCDebug(JoystickLog) << "setButtonPwm: " << button << " has same action as " << anyOtherButtonWithSameAction << " setting low pwm to " << value; + //TODO(bzd) emit error + } + } _buttonActionArray[button]->lowPwm(value); settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value); } else { _buttonActionArray[button]->highPwm(value); settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value); } + return value; } + return -1; } int Joystick::getButtonPwm(int button, bool lowPwm) { - if (_validButton(button)) { - if (assignableButtonActionIsPwm(button)) { - QSettings settings; - settings.beginGroup(_settingsGroup); - settings.beginGroup(_name); - if (lowPwm) { - return settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); - } else { - return settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); - } + if (_validButton(button) && assignableButtonActionIsPwm(button)) { + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if (lowPwm) { + return settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt(); + } else { + return settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt(); } } + return -1; } @@ -1413,4 +1423,17 @@ uint16_t Joystick::_mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value) { return rcChannel < 9 ? 0 : UINT16_MAX - 1; } return value; +} + +int Joystick::_getOtherMultiButtonPWMOverrideButtonIndex(int button) { + if (_buttonActionArray[button] && _buttonActionArray[button]->isPwmOverrideAction()) { + auto action = _buttonActionArray[button]->action(); + // check if there is another button with the same action + for (int i = 0; i < _buttonActionArray.count(); i++) { + if (i != button && _buttonActionArray[i] && _buttonActionArray[i]->action() == action) { + return i; + } + } + } + return -1; } \ No newline at end of file diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index ecd433983c1..f32dbd3b28b 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -157,7 +157,7 @@ class Joystick : public QThread Q_INVOKABLE QString getButtonAction (int button); Q_INVOKABLE bool assignableButtonActionIsPwm (int button); Q_INVOKABLE bool assignableActionIsPwm (QString action); - Q_INVOKABLE void setButtonPwm (int button, bool lowPwm, int value); + Q_INVOKABLE int setButtonPwm (int button, bool lowPwm, int value); Q_INVOKABLE int getButtonPwm (int button, bool lowPwm); // Property accessors @@ -294,7 +294,6 @@ class Joystick : public QThread int _mapFunctionMode(int mode, int function); void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); - //TODO(bzd) change private members to use _ prefix void _removeButtonSettings(int button); void _saveButtonSettings(int button); /// if repeat is available for action under button, sets passed repeat flag @@ -303,6 +302,18 @@ class Joystick : public QThread bool _executeRcOverrideButtonAction(int buttonIndex, bool buttonDown); void _clearRcOverrideButtonActions(); uint16_t _mapRcOverrideToRelease(uint8_t rcChannel, uint16_t value); +// bool _isButtonOfMultiButtonPWMOverride(int button); + /** + * @brief Checks if the button is a multi-button PWM override, returns any other button if it is + * + * Searches for other buttons that are part of the same multi-button PWM override Action and returns the first one found that is different from the passed button. + * If the passed button is not part of a multi-button PWM override, returns -1. + * + * @param button + * @return + */ + int _getOtherMultiButtonPWMOverrideButtonIndex(int button); + bool _isActionMultiButtonPWMOverride(const QString& action); // Override from QThread virtual void run(); diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml index 4900a5d260e..e26141785a0 100644 --- a/src/VehicleSetup/JoystickConfigButtons.qml +++ b/src/VehicleSetup/JoystickConfigButtons.qml @@ -119,6 +119,7 @@ ColumnLayout { visible: _activeJoystick ? _activeJoystick.pwmVisibilities[modelData] : false function _setButtonPwm(button, isLow, pwm) { + var pwmValue = -1; if(_activeJoystick) { if (pwm < 1000) { pwm = 1000; @@ -126,16 +127,17 @@ ColumnLayout { if (pwm > 2000) { pwm = 2000; } - _activeJoystick.setButtonPwm(modelData, isLow, pwm) + pwmValue = _activeJoystick.setButtonPwm(modelData, isLow, pwm) } + return pwmValue == -1 ? "" : pwmValue; } function _getButtonPwm(button, isLow) { - var pwm = -1; + var pwmValue = -1; if(_activeJoystick) { - pwm = _activeJoystick.getButtonPwm(modelData, isLow) + pwmValue = _activeJoystick.getButtonPwm(modelData, isLow) } - return pwm == -1 ? "" : pwm; + return pwmValue == -1 ? "" : pwmValue; } QGCLabel { @@ -143,6 +145,7 @@ ColumnLayout { text: qsTr("Low") anchors.verticalCenter: parent.verticalCenter } + QGCTextField { id: lowPwmValue width: ScreenTools.defaultFontPixelWidth * 10 @@ -153,7 +156,7 @@ ColumnLayout { Connections { target: buttonActionCombo onCurrentIndexChanged: { - if(_activeJoystick) { + if (_activeJoystick) { console.log("index changed, ", buttonActionCombo.currentIndex) console.log("index changed, ", modelData) console.log("index changed, ", target) @@ -166,10 +169,14 @@ ColumnLayout { Component.onCompleted: { if(_activeJoystick) { - text = parent._getButtonPwm(modelData, true) + text = pwmSettings._getButtonPwm(modelData, true) } } - onEditingFinished: parent._setButtonPwm(modelData, true, text) + onEditingFinished: { + // setButtonPwm calculates proper value and we set it back + var pwm = pwmSettings._setButtonPwm(modelData, true, text) + lowPwmValue.text = pwm; + } } QGCLabel { @@ -201,15 +208,21 @@ ColumnLayout { Component.onCompleted: { if(_activeJoystick) { - text = parent._getButtonPwm(modelData, false) + text = pwmSettings._getButtonPwm(modelData, false) } } - onEditingFinished: parent._setButtonPwm(modelData, false, text) + onEditingFinished: { + // setButtonPwm calculates proper value and we set it back + var pwm = pwmSettings._setButtonPwm(modelData, false, text) + highPwmValue.text = pwm; + } } + QGCCheckBox { id: latchCheck text: qsTr("Latch") anchors.verticalCenter: parent.verticalCenter + enabled: pwmSettings._latchEnabled(modelData) onClicked: { _activeJoystick.setButtonPwmLatch(modelData, checked) From 99cbbbd91eccce27e9333e51d26b43bb2b419caa Mon Sep 17 00:00:00 2001 From: Zdanek Date: Fri, 4 Aug 2023 14:21:33 +0200 Subject: [PATCH 11/11] Cmake fix for compiling APMRemoteSupportComponent --- src/AutoPilotPlugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/AutoPilotPlugins/CMakeLists.txt b/src/AutoPilotPlugins/CMakeLists.txt index 54d40734819..774c383423e 100644 --- a/src/AutoPilotPlugins/CMakeLists.txt +++ b/src/AutoPilotPlugins/CMakeLists.txt @@ -65,6 +65,7 @@ if (NOT QGC_DISABLE_APM_PLUGIN) APM/APMSubFrameComponent.cc APM/APMSubMotorComponentController.cc APM/APMTuningComponent.cc + APM/APMRemoteSupportComponent.cc ) endif()