From 2d816e0b3ee741fadeceef53220571f2bfea7f5d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Apr 2021 13:51:52 -0400 Subject: [PATCH] [WIP] manual_control selector hacks --- ROMFS/px4fmu_common/init.d/rcS | 2 + boards/airmind/mindpx-v2/default.px4board | 1 + boards/atl/mantis-edu/default.px4board | 1 + boards/av/x-v1/default.px4board | 1 + boards/beaglebone/blue/default.px4board | 1 + boards/bitcraze/crazyflie/default.px4board | 1 + boards/bitcraze/crazyflie21/default.px4board | 1 + boards/cuav/nora/default.px4board | 1 + boards/cuav/x7pro/default.px4board | 1 + boards/cubepilot/cubeorange/default.px4board | 1 + boards/cubepilot/cubeyellow/default.px4board | 1 + boards/emlid/navio2/default.px4board | 1 + boards/holybro/durandal-v1/default.px4board | 1 + boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/pix32v5/default.px4board | 1 + boards/modalai/fc-v1/default.px4board | 1 + boards/modalai/fc-v2/default.px4board | 1 + boards/mro/ctrl-zero-f7-oem/default.px4board | 1 + boards/mro/ctrl-zero-f7/default.px4board | 1 + boards/mro/ctrl-zero-h7-oem/default.px4board | 1 + boards/mro/ctrl-zero-h7/default.px4board | 1 + boards/mro/pixracerpro/default.px4board | 1 + boards/mro/x21-777/default.px4board | 1 + boards/mro/x21/default.px4board | 1 + boards/nxp/fmuk66-e/default.px4board | 1 + boards/nxp/fmuk66-v3/default.px4board | 1 + boards/nxp/fmurt1062-v1/default.px4board | 1 + boards/omnibus/f4sd/default.px4board | 1 + boards/px4/fmu-v2/default.px4board | 1 + boards/px4/fmu-v3/default.px4board | 1 + boards/px4/fmu-v4/default.px4board | 1 + boards/px4/fmu-v4pro/default.px4board | 1 + boards/px4/fmu-v5/default.px4board | 1 + boards/px4/fmu-v5x/default.px4board | 1 + boards/px4/fmu-v6u/default.px4board | 1 + boards/px4/fmu-v6x/default.px4board | 1 + boards/px4/raspberrypi/default.px4board | 1 + boards/px4/sitl/default.px4board | 1 + boards/scumaker/pilotpi/default.px4board | 1 + boards/spracing/h7extreme/default.px4board | 1 + boards/uvify/core/default.px4board | 1 + msg/CMakeLists.txt | 1 + msg/manual_control_input.msg | 52 ++++ msg/manual_control_setpoint.msg | 23 +- src/modules/manual_control/CMakeLists.txt | 44 ++++ src/modules/manual_control/Kconfig | 5 + src/modules/manual_control/ManualControl.cpp | 249 +++++++++++++++++++ src/modules/manual_control/ManualControl.hpp | 115 +++++++++ src/modules/mavlink/mavlink_main.cpp | 2 - src/modules/mavlink/mavlink_main.h | 18 -- src/modules/mavlink/mavlink_receiver.cpp | 62 +---- src/modules/mavlink/mavlink_receiver.h | 4 +- src/modules/rc_update/rc_update.cpp | 64 ++--- src/modules/rc_update/rc_update.h | 8 +- 54 files changed, 574 insertions(+), 115 deletions(-) create mode 100644 msg/manual_control_input.msg create mode 100644 src/modules/manual_control/CMakeLists.txt create mode 100644 src/modules/manual_control/Kconfig create mode 100644 src/modules/manual_control/ManualControl.cpp create mode 100644 src/modules/manual_control/ManualControl.hpp diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 45f9bd7561..540929e8cb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -326,6 +326,8 @@ else # rc_update start + manual_control start + # # Sensors System (start before Commander so Preflight checks are properly run). # Commander needs to be this early for in-air-restarts. diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 98fab53d38..aa136f13b8 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -58,6 +58,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board index 846e9c9b50..767737751a 100644 --- a/boards/atl/mantis-edu/default.px4board +++ b/boards/atl/mantis-edu/default.px4board @@ -17,6 +17,7 @@ CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 2b46215968..0c642b4687 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index ddb77ae9d7..497f7d0c45 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -39,6 +39,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 5c440a5f27..cb6dac50ca 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -21,6 +21,7 @@ CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 4f0bd50b12..53a9f79d30 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -21,6 +21,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index f3e1983c9d..ecfee0997a 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 0151d294cd..c16d37046c 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -64,6 +64,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 1f7167d6bf..70f6452542 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -59,6 +59,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index 59533de53c..716f499b0d 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -58,6 +58,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index f89f5c7744..3d04004c35 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index a32011a835..4071f15b8d 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index e0a1209f6b..8e2e299153 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -27,6 +27,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 5e6eb1af6d..f545b7c0d0 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 3b003e71eb..4ffe7fb4a0 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -58,6 +58,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index 661fb60524..63024ca9f2 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index ee0f095f91..9e41e05e3e 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index 68889ddfc4..8f6433de50 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index e47e1bfe92..633608ece1 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 9ecf05e07c..0ccd772098 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index 8af63c2326..0c70fe2c96 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -55,6 +55,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index f9062a3968..da0515d7af 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -58,6 +58,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 03d31b60bc..cdc58154ca 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -59,6 +59,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 568b0ded9b..77607330c7 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -58,6 +58,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index ab96adab0b..c41df2c0dd 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index bedc8342c8..44943481b3 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -36,6 +36,7 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 38e1d573a8..3b0dd47b02 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -28,6 +28,7 @@ CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index e9af13b3ba..a25e761aad 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -31,6 +31,7 @@ CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 7110fa2d47..52f4a4bdf4 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index ba7f838497..552d9c70d7 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index 6c65dd2982..1885d3ed11 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -60,6 +60,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index d876f4cb89..d0cbcdc12e 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -65,6 +65,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 522fcc208e..d305af43aa 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -67,6 +67,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index d88f9f399f..b31f58fd30 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -59,6 +59,7 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index b5ff8eed18..648dd8fa6d 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 2338f334a7..98deabed76 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -39,6 +39,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 09e153731f..47a4ec17c2 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -27,6 +27,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index ead27d50a7..a600b40cce 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -39,6 +39,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index 16af5a20d3..b0eee0cdc0 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -28,6 +28,7 @@ CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/uvify/core/default.px4board b/boards/uvify/core/default.px4board index cfdc6015d2..7d8421ffe0 100644 --- a/boards/uvify/core/default.px4board +++ b/boards/uvify/core/default.px4board @@ -46,6 +46,7 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 10931d5ca2..a343062b64 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -102,6 +102,7 @@ set(msg_files logger_status.msg mag_worker_data.msg magnetometer_bias_estimate.msg + manual_control_input.msg manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg diff --git a/msg/manual_control_input.msg b/msg/manual_control_input.msg new file mode 100644 index 0000000000..166a28c2f4 --- /dev/null +++ b/msg/manual_control_input.msg @@ -0,0 +1,52 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) +uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 +uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 +uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 + +uint8 data_source + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. +# The variable names follow the definition of the +# MANUAL_CONTROL mavlink message. +# The default range is from -1 to 1 (mavlink message -1000 to 1000) +# The range for the z variable is defined from 0 to 1. (The z field of +# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) + +float32 x # stick position in x direction -1..1 + # in general corresponds to forward/back motion or pitch of vehicle, + # in general a positive value means forward or negative pitch and + # a negative value means backward or positive pitch + +float32 y # stick position in y direction -1..1 + # in general corresponds to right/left motion or roll of vehicle, + # in general a positive value means right or positive roll and + # a negative value means left or negative roll + +float32 z # throttle stick position 0..1 + # in general corresponds to up/down motion or thrust of vehicle, + # in general the value corresponds to the demanded throttle by the user, + # if the input is used for setting the setpoint of a vertical position + # controller any value > 0.5 means up and any value < 0.5 means down + +float32 r # yaw stick/twist position, -1..1 + # in general corresponds to the righthand rotation around the vertical + # (downwards) axis of the vehicle + +float32 flaps # flap position + +float32 aux1 # default function: camera yaw / azimuth +float32 aux2 # default function: camera pitch / tilt +float32 aux3 # default function: camera trigger +float32 aux4 # default function: camera roll +float32 aux5 # default function: payload drop +float32 aux6 + diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 35ad76f70b..a39e1f6911 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -1,14 +1,17 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) -uint8 SOURCE_RC = 1 # radio control +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 -uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 4 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 -uint8 data_source # where this input is coming from +uint8 data_source # Any of the channels may not be available and be set to NaN # to indicate that it does not contain valid data. @@ -38,6 +41,12 @@ float32 r # yaw stick/twist position, -1..1 # in general corresponds to the righthand rotation around the vertical # (downwards) axis of the vehicle +# stick velocity +float32 vx +float32 vy +float32 vz +float32 vr + float32 flaps # flap position float32 aux1 @@ -47,3 +56,7 @@ float32 aux4 float32 aux5 float32 aux6 +bool arm_gesture +bool disarm_gesture + +bool user_override diff --git a/src/modules/manual_control/CMakeLists.txt b/src/modules/manual_control/CMakeLists.txt new file mode 100644 index 0000000000..9a6652768a --- /dev/null +++ b/src/modules/manual_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE module__manual_control + MAIN manual_control + COMPILE_FLAGS + SRCS + ManualControl.cpp + ManualControl.cpp + ManualControl.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/manual_control/Kconfig b/src/modules/manual_control/Kconfig new file mode 100644 index 0000000000..f5582f8761 --- /dev/null +++ b/src/modules/manual_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_MANUAL_CONTROL + bool "manual_control" + default n + ---help--- + Enable support for manual_control diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp new file mode 100644 index 0000000000..b4fd18f53e --- /dev/null +++ b/src/modules/manual_control/ManualControl.cpp @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ManualControl.hpp" + +#include + +namespace manual_control +{ + +ManualControl::ManualControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +ManualControl::~ManualControl() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +bool ManualControl::init() +{ + ScheduleNow(); + return true; +} + +void ManualControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); + _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); + } + + const hrt_abstime rc_timeout = _param_com_rc_loss_t.get() * 1_s; + + bool publish_once = false; + int selected_manual_input = -1; + + for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { + manual_control_input_s manual_control_input; + + if (_manual_control_input_subs[i].update(&manual_control_input)) { + + bool publish = false; + + if ((_param_com_rc_in_mode.get() == 0) + && (manual_control_input.data_source == manual_control_input_s::SOURCE_RC)) { + + publish = true; + + } else if ((_param_com_rc_in_mode.get() == 1) + && (manual_control_input.data_source >= manual_control_input_s::SOURCE_MAVLINK_0)) { + + publish = true; + + } else { + // otherwise, first come, first serve (REVIEW) + publish = true; + } + + + bool available = (hrt_elapsed_time(&manual_control_input.timestamp) < rc_timeout) + && (hrt_elapsed_time(&_manual_control_input[i].timestamp) < rc_timeout); + + if (publish && available && !publish_once) { + const float dt_inv = 1e6f / static_cast(manual_control_input.timestamp_sample - + _manual_control_input[i].timestamp_sample); + + manual_control_setpoint_s manual_control_setpoint{}; + + manual_control_setpoint.timestamp_sample = manual_control_input.timestamp_sample; + manual_control_setpoint.x = manual_control_input.x; + manual_control_setpoint.y = manual_control_input.y; + manual_control_setpoint.z = manual_control_input.z; + manual_control_setpoint.r = manual_control_input.r; + manual_control_setpoint.vx = (manual_control_input.x - _manual_control_input[i].x) * dt_inv; + manual_control_setpoint.vy = (manual_control_input.y - _manual_control_input[i].y) * dt_inv; + manual_control_setpoint.vz = (manual_control_input.z - _manual_control_input[i].z) * dt_inv; + manual_control_setpoint.vr = (manual_control_input.r - _manual_control_input[i].r) * dt_inv; + manual_control_setpoint.flaps = manual_control_input.flaps; + manual_control_setpoint.aux1 = manual_control_input.aux1; + manual_control_setpoint.aux2 = manual_control_input.aux2; + manual_control_setpoint.aux3 = manual_control_input.aux3; + manual_control_setpoint.aux4 = manual_control_input.aux4; + manual_control_setpoint.aux5 = manual_control_input.aux5; + manual_control_setpoint.aux6 = manual_control_input.aux6; + manual_control_setpoint.data_source = manual_control_input.data_source; + + + // user arm/disarm gesture + const bool right_stick_centered = (fabsf(manual_control_input.x) < 0.1f) && (fabsf(manual_control_input.y) < 0.1f); + const bool stick_lower_left = (manual_control_input.z < 0.1f) && (manual_control_input.r < -0.9f); + const bool stick_lower_right = (manual_control_input.z < 0.1f) && (manual_control_input.r > 0.9f); + + _stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, manual_control_input.timestamp); + _stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, manual_control_input.timestamp); + manual_control_setpoint.arm_gesture = _stick_arm_hysteresis.get_state(); + manual_control_setpoint.disarm_gesture = _stick_disarm_hysteresis.get_state(); + + + // user wants override + const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); + const bool rpy_moved = (fabsf(manual_control_input.x - _manual_control_input[i].x) > minimum_stick_change) + || (fabsf(manual_control_input.y - _manual_control_input[i].y) > minimum_stick_change) + || (fabsf(manual_control_input.r - _manual_control_input[i].r) > minimum_stick_change); + + // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] + const bool throttle_moved = (fabsf(manual_control_input.z - _manual_control_input[i].z) * 2.f > minimum_stick_change); + const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + + manual_control_setpoint.user_override = rpy_moved || (use_throttle && throttle_moved); + + + manual_control_setpoint.timestamp = hrt_absolute_time(); + _manual_control_setpoint_pub.publish(manual_control_setpoint); + publish_once = true; + + selected_manual_input = i; + } + + _manual_control_input[i] = manual_control_input; + _available[i] = available; + } + } + + if ((selected_manual_input >= 0) && (selected_manual_input != _selected_manual_input)) { + if (_selected_manual_input >= 0) { + PX4_INFO("selected manual_control_input changed %d -> %d", _selected_manual_input, selected_manual_input); + } + + _manual_control_input_subs[selected_manual_input].registerCallback(); + _selected_manual_input = selected_manual_input; + } + + // reschedule timeout + ScheduleDelayed(200_ms); + + perf_end(_loop_perf); +} + +int ManualControl::task_spawn(int argc, char *argv[]) +{ + ManualControl *instance = new ManualControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ManualControl::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int ManualControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ManualControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("manual_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +}; // namespace manual_control + +extern "C" __EXPORT int manual_control_main(int argc, char *argv[]) +{ + return manual_control::ManualControl::main(argc, argv); +} diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp new file mode 100644 index 0000000000..5f9a806887 --- /dev/null +++ b/src/modules/manual_control/ManualControl.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace manual_control +{ + +class ManualControl : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + ManualControl(); + ~ManualControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + int print_status() override; + +private: + static constexpr int MAX_MANUAL_INPUT_COUNT = 3; + + + enum OverrideBits { + OVERRIDE_AUTO_MODE_BIT = (1 << 0), + OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), + OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) + }; + + void Run() override; + + uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] { + {this, ORB_ID(manual_control_input), 0}, + {this, ORB_ID(manual_control_input), 1}, + {this, ORB_ID(manual_control_input), 2}, + }; + + manual_control_input_s _manual_control_input[MAX_MANUAL_INPUT_COUNT] {}; + + bool _available[MAX_MANUAL_INPUT_COUNT] {}; + + int8_t _selected_manual_input{-1}; + + systemlib::Hysteresis _stick_arm_hysteresis{false}; + systemlib::Hysteresis _stick_disarm_hysteresis{false}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_com_rc_in_mode, + (ParamFloat) _param_com_rc_loss_t, + (ParamInt) _param_rc_override, + (ParamFloat) _param_com_rc_stick_ov, + (ParamInt) _param_rc_arm_hyst + ) +}; +}; // namespace manual_control diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e69b05fc53..1960c84e42 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2315,8 +2315,6 @@ Mavlink::task_main(int argc, char *argv[]) /* switch HIL mode if required */ set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); - set_generate_virtual_rc_input(vehicle_status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); - if (_mode == MAVLINK_MODE_IRIDIUM) { if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d17f44db8a..fc536fe40d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -318,28 +318,11 @@ public: */ int set_hil_enabled(bool hil_enabled); - /** - * Set manual input generation mode - * - * Set to true to generate RC_INPUT messages on the system bus from - * MAVLink messages. - * - * @param generation_enabled If set to true, generate RC_INPUT messages - */ - void set_generate_virtual_rc_input(bool generation_enabled) { _generate_rc = generation_enabled; } - /** * Set communication protocol for this mavlink instance */ void set_protocol(Protocol p) { _protocol = p; } - /** - * Get the manual input generation mode - * - * @return true if manual inputs should generate RC data - */ - bool should_generate_virtual_rc_input() { return _generate_rc; } - /** * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction */ @@ -573,7 +556,6 @@ private: /* states */ bool _hil_enabled{false}; /**< Hardware In the Loop mode */ - bool _generate_rc{false}; /**< Generate RC messages from manual input MAVLink messages */ bool _is_usb_uart{false}; /**< Port is USB */ bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */ bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d47c5e296a..17bd3f67e0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2069,16 +2069,14 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) // fill uORB message input_rc_s rc{}; - // metadata rc.timestamp = hrt_absolute_time(); - rc.timestamp_last_signal = rc.timestamp; + rc.timestamp_last_signal = hrt_absolute_time(); rc.rssi = input_rc_s::RSSI_MAX; rc.rc_failsafe = false; rc.rc_lost = false; rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; - rc.rc_ppm_frame_length = 0; rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; // channels @@ -2132,55 +2130,15 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) return; } - if (_mavlink->should_generate_virtual_rc_input()) { - - input_rc_s rc{}; - rc.timestamp = hrt_absolute_time(); - rc.timestamp_last_signal = rc.timestamp; - - rc.channel_count = 8; - rc.rc_failsafe = false; - rc.rc_lost = false; - rc.rc_lost_frame_count = 0; - rc.rc_total_frame_count = 1; - rc.rc_ppm_frame_length = 0; - rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; - rc.rssi = input_rc_s::RSSI_MAX; - - rc.values[0] = man.x / 2 + 1500; // roll - rc.values[1] = man.y / 2 + 1500; // pitch - rc.values[2] = man.r / 2 + 1500; // yaw - rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle - - /* decode all switches which fit into the channel mask */ - unsigned max_switch = (sizeof(man.buttons) * 8); - unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0])); - - if (max_switch > (max_channels - 4)) { - max_switch = (max_channels - 4); - } - - /* fill all channels */ - for (unsigned i = 0; i < max_switch; i++) { - rc.values[i + 4] = decode_switch_pos_n(man.buttons, i); - } - - _mom_switch_state = man.buttons; - - _rc_pub.publish(rc); - - } else { - manual_control_setpoint_s manual{}; - - manual.timestamp = hrt_absolute_time(); - manual.x = man.x / 1000.0f; - manual.y = man.y / 1000.0f; - manual.r = man.r / 1000.0f; - manual.z = man.z / 1000.0f; - manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - - _manual_control_setpoint_pub.publish(manual); - } + manual_control_input_s manual{}; + manual.timestamp_sample = hrt_absolute_time(); + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; + manual.data_source = manual_control_input_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); + manual.timestamp = hrt_absolute_time(); + _manual_control_input_pub.publish(manual); } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fd06db3d69..afba64628e 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -80,7 +80,7 @@ #include #include #include -#include +#include #include #include #include @@ -331,7 +331,7 @@ private: uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 0d2e302366..135bf4eba5 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -526,7 +526,7 @@ void RCUpdate::Run() } // limit processing if there's no update - if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) { + if (rc_updated || (hrt_elapsed_time(&_last_manual_control_input_publish) > 300_ms)) { UpdateManualSetpoint(input_rc.timestamp_last_signal); } @@ -661,43 +661,43 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) { - manual_control_setpoint_s manual_control_setpoint{}; - manual_control_setpoint.timestamp_sample = timestamp_sample; - manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC; + manual_control_input_s manual_control_input{}; + manual_control_input.timestamp_sample = timestamp_sample; + manual_control_input.data_source = manual_control_input_s::SOURCE_RC; // limit controls - manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); - manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); - manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); - manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); - manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); - manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); - manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); - manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f); - manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f); - manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f); - manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f); + manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); + manual_control_input.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); + manual_control_input.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); + manual_control_input.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); + manual_control_input.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); + manual_control_input.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); + manual_control_input.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); + manual_control_input.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f); + manual_control_input.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f); + manual_control_input.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f); + manual_control_input.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f); - // publish manual_control_setpoint topic - manual_control_setpoint.timestamp = hrt_absolute_time(); - _manual_control_setpoint_pub.publish(manual_control_setpoint); - _last_manual_control_setpoint_publish = manual_control_setpoint.timestamp; + // publish manual_control_input topic + manual_control_input.timestamp = hrt_absolute_time(); + _manual_control_input_pub.publish(manual_control_input); + _last_manual_control_input_publish = manual_control_input.timestamp; actuator_controls_s actuator_group_3{}; // copy in previous actuator control setpoint in case aux{1, 2, 3} isn't changed _actuator_controls_3_sub.update(&actuator_group_3); - // populate and publish actuator_controls_3 copied from mapped manual_control_setpoint - actuator_group_3.control[0] = manual_control_setpoint.y; - actuator_group_3.control[1] = manual_control_setpoint.x; - actuator_group_3.control[2] = manual_control_setpoint.r; - actuator_group_3.control[3] = manual_control_setpoint.z; - actuator_group_3.control[4] = manual_control_setpoint.flaps; + // populate and publish actuator_controls_3 copied from mapped manual_control_input + actuator_group_3.control[0] = manual_control_input.y; + actuator_group_3.control[1] = manual_control_input.x; + actuator_group_3.control[2] = manual_control_input.r; + actuator_group_3.control[3] = manual_control_input.z; + actuator_group_3.control[4] = manual_control_input.flaps; float new_aux_values[3]; - new_aux_values[0] = manual_control_setpoint.aux1; - new_aux_values[1] = manual_control_setpoint.aux2; - new_aux_values[2] = manual_control_setpoint.aux3; + new_aux_values[0] = manual_control_input.aux1; + new_aux_values[1] = manual_control_input.aux2; + new_aux_values[2] = manual_control_input.aux3; // if AUX RC was already active, we update. otherwise, we check // if there is a major stick movement to re-activate RC mode @@ -705,7 +705,7 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) // detect a big stick movement for (int i = 0; i < 3; i++) { - if (fabsf(_last_manual_control_setpoint[i] - new_aux_values[i]) > 0.1f) { + if (fabsf(_last_manual_control_input[i] - new_aux_values[i]) > 0.1f) { major_movement[i] = true; } } @@ -713,14 +713,14 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) for (int i = 0; i < 3; i++) { // if someone else (DO_SET_ACTUATOR) updated the actuator control // and we haven't had a major movement, switch back to automatic control - if ((fabsf(_last_manual_control_setpoint[i] - actuator_group_3.control[5 + i]) + if ((fabsf(_last_manual_control_input[i] - actuator_group_3.control[5 + i]) > 0.0001f) && (!major_movement[i])) { _aux_already_active[i] = false; } if (_aux_already_active[i] || major_movement[i]) { _aux_already_active[i] = true; - _last_manual_control_setpoint[i] = new_aux_values[i]; + _last_manual_control_input[i] = new_aux_values[i]; actuator_group_3.control[5 + i] = new_aux_values[i]; } @@ -789,7 +789,7 @@ int RCUpdate::print_usage(const char *reason) ### Description The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`), then apply the calibration, map the RC channels to the configured channels & mode switches -and then publish as `rc_channels` and `manual_control_setpoint`. +and then publish as `rc_channels` and `manual_control_input`. ### Implementation To reduce control latency, the module is scheduled on input_rc publications. diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index a21230036f..5039fad67a 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -166,7 +166,7 @@ private: uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; uORB::Publication _rc_channels_pub{ORB_ID(rc_channels)}; - uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::Publication _manual_control_switches_pub{ORB_ID(manual_control_switches)}; uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; @@ -177,12 +177,12 @@ private: rc_parameter_map_s _rc_parameter_map {}; float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */ - hrt_abstime _last_manual_control_setpoint_publish{0}; + hrt_abstime _last_manual_control_input_publish{0}; hrt_abstime _last_rc_to_param_map_time{0}; hrt_abstime _last_timestamp_signal{0}; uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {}; - float _last_manual_control_setpoint[3] {}; + float _last_manual_control_input[3] {}; bool _aux_already_active[3] = {false, false, false}; uint8_t _channel_count_previous{0};