From a471755dac81df146c0aa547b9eb8eaafe01f9f3 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 19 Dec 2023 15:04:34 +0100 Subject: [PATCH] Rover module: Temp --- .../init.d-posix/airframes/CMakeLists.txt | 1 + .../init.d/rc.rover_differential_apps | 2 +- .../simulation/.vscode/c_cpp_properties.json | 20 +++ Tools/simulation/.vscode/settings.json | 10 ++ boards/airmind/mindpx-v2/default.px4board | 2 +- boards/ark/fmu-v6x/default.px4board | 2 +- boards/av/x-v1/default.px4board | 2 +- boards/beaglebone/blue/default.px4board | 2 +- boards/cuav/nora/default.px4board | 2 +- boards/cuav/x7pro/default.px4board | 2 +- boards/cubepilot/cubeyellow/default.px4board | 2 +- boards/emlid/navio2/default.px4board | 2 +- boards/holybro/durandal-v1/default.px4board | 2 +- boards/holybro/kakuteh7/default.px4board | 2 +- boards/holybro/kakuteh7mini/default.px4board | 2 +- boards/holybro/kakuteh7v2/default.px4board | 2 +- boards/holybro/pix32v5/default.px4board | 2 +- boards/modalai/fc-v1/default.px4board | 2 +- boards/mro/ctrl-zero-classic/default.px4board | 2 +- boards/mro/ctrl-zero-f7-oem/default.px4board | 2 +- boards/mro/ctrl-zero-f7/default.px4board | 2 +- boards/mro/ctrl-zero-h7-oem/default.px4board | 2 +- boards/mro/ctrl-zero-h7/default.px4board | 2 +- boards/mro/pixracerpro/default.px4board | 2 +- boards/mro/x21-777/default.px4board | 2 +- boards/mro/x21/default.px4board | 2 +- boards/nxp/fmuk66-e/default.px4board | 2 +- boards/nxp/fmuk66-v3/default.px4board | 2 +- boards/nxp/mr-canhubk3/fmu.px4board | 2 +- boards/nxp/mr-canhubk3/sysview.px4board | 2 +- boards/nxp/mr-canhubk3/zenoh.px4board | 2 +- boards/px4/fmu-v2/rover.px4board | 2 +- boards/px4/fmu-v3/default.px4board | 2 +- boards/px4/fmu-v4/default.px4board | 2 +- boards/px4/fmu-v4pro/default.px4board | 2 +- boards/px4/fmu-v5/default.px4board | 2 +- boards/px4/fmu-v5x/default.px4board | 2 +- boards/px4/fmu-v6c/default.px4board | 2 +- boards/px4/fmu-v6u/default.px4board | 2 +- boards/px4/fmu-v6xrt/default.px4board | 2 +- boards/px4/raspberrypi/default.px4board | 2 +- boards/px4/sitl/default.px4board | 2 +- boards/scumaker/pilotpi/default.px4board | 2 +- boards/siyi/n7/default.px4board | 2 +- boards/thepeach/k1/default.px4board | 2 +- boards/thepeach/r1/default.px4board | 2 +- .../control_allocator/ControlAllocator.cpp | 2 +- .../CMakeLists.txt | 6 +- .../DifferentialDriveControl.cpp | 40 +++--- .../DifferentialDriveControl.hpp | 4 +- .../CMakeLists.txt | 0 .../DifferentialDriveKinematics.cpp | 0 .../DifferentialDriveKinematics.hpp | 0 .../DifferentialDriveKinematicsTest.cpp | 0 .../Kconfig | 4 +- src/modules/rover_control/RoverControl.cpp | 122 ++++++++++++++++++ src/modules/rover_control/RoverControl.hpp | 118 +++++++++++++++++ .../module.yaml | 0 ...ceMotor.cpp => GZMixingInterfaceWheel.cpp} | 0 ...ceMotor.hpp => GZMixingInterfaceWheel.hpp} | 0 60 files changed, 347 insertions(+), 66 deletions(-) create mode 100644 Tools/simulation/.vscode/c_cpp_properties.json create mode 100644 Tools/simulation/.vscode/settings.json rename src/modules/{differential_drive_control => rover_control}/CMakeLists.txt (95%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveControl.cpp (90%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveControl.hpp (98%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveKinematics/CMakeLists.txt (100%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp (100%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp (100%) rename src/modules/{differential_drive_control => rover_control}/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp (100%) rename src/modules/{differential_drive_control => rover_control}/Kconfig (59%) create mode 100644 src/modules/rover_control/RoverControl.cpp create mode 100644 src/modules/rover_control/RoverControl.hpp rename src/modules/{differential_drive_control => rover_control}/module.yaml (100%) rename src/modules/simulation/gz_bridge/{GZMixingInterfaceMotor.cpp => GZMixingInterfaceWheel.cpp} (100%) rename src/modules/simulation/gz_bridge/{GZMixingInterfaceMotor.hpp => GZMixingInterfaceWheel.hpp} (100%) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 247545c187..ed460d9929 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -80,6 +80,7 @@ px4_add_romfs_files( 4005_gz_x500_vision 4006_gz_px4vision 4008_gz_advanced_plane + 4009_gz_r1_rover 4010_gz_x500_mono_cam 6011_gazebo-classic_typhoon_h480 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 31c79ac980..2be62a3169 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -5,7 +5,7 @@ ekf2 start & # Start rover differential drive controller. -differential_drive_control start +rover_control start # Start Land Detector. land_detector start rover diff --git a/Tools/simulation/.vscode/c_cpp_properties.json b/Tools/simulation/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..af5a8df28d --- /dev/null +++ b/Tools/simulation/.vscode/c_cpp_properties.json @@ -0,0 +1,20 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/Tools/simulation/.vscode/settings.json b/Tools/simulation/.vscode/settings.json new file mode 100644 index 0000000000..593784e4f1 --- /dev/null +++ b/Tools/simulation/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index edeae788a4..889132b559 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index be5f90af2b..cf9f3d8fee 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 241097d340..60f2ccf875 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -36,7 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 9794e54e37..1d562d44dc 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -25,7 +25,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index 7029202ea8..7c70b3cdae 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 923cfa5930..f82acafe35 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index e35ee1926c..b3e9924ab7 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 9058f52517..3ea2ee5d96 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -27,7 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index 39337f84f6..b5c37b66c5 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 695864f886..d2482b622a 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -32,7 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index aae190b1a3..60cffe3a90 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index e28e21b691..7194c8c232 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -32,7 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index e1fded8f64..e0033e0956 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 0633173105..6d03ed5a4b 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -41,7 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index 361ab45eab..ac2be5b97c 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -36,7 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index 3bef218d77..8669d5b03c 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index 0b9907c4c6..b69dd10bae 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 74899cfe4e..2805f85b30 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -35,7 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 4dffec7334..9862964b19 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -36,7 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index e90444a6d7..0077777889 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -35,7 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 68bc757dc6..c05f2f5bf7 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -38,7 +38,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 3a369dccb2..ea551f3cee 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 87ab171e1e..30f7c5462f 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -40,7 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index db5b9656df..55390f9018 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -41,7 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index 0dcccbb247..ad67ca13a6 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -28,7 +28,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/nxp/mr-canhubk3/sysview.px4board b/boards/nxp/mr-canhubk3/sysview.px4board index 4ab083778d..4628eff9a5 100644 --- a/boards/nxp/mr-canhubk3/sysview.px4board +++ b/boards/nxp/mr-canhubk3/sysview.px4board @@ -15,7 +15,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/nxp/mr-canhubk3/zenoh.px4board b/boards/nxp/mr-canhubk3/zenoh.px4board index 7ea03afb08..91a3f2826d 100644 --- a/boards/nxp/mr-canhubk3/zenoh.px4board +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -20,7 +20,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/boards/px4/fmu-v2/rover.px4board b/boards/px4/fmu-v2/rover.px4board index 99f121b554..d77caf9e9c 100644 --- a/boards/px4/fmu-v2/rover.px4board +++ b/boards/px4/fmu-v2/rover.px4board @@ -1,4 +1,4 @@ -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=n CONFIG_MODULES_MC_ATT_CONTROL=n CONFIG_MODULES_MC_POS_CONTROL=n diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 68bf610868..c15b1c8758 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index 14d84a229e..6ebd89d931 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index 726b3c2b11..8025c118d7 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -40,7 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 4235576616..0094e91852 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -48,7 +48,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 877e2d2e25..ad19de12ff 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index a5408c6d85..9d673b2eb4 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index d76fff66bc..c57508575b 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -40,7 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index bb21b6c1c3..a6a4965a36 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 8a0110b21d..90455655a8 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -24,7 +24,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 008dc97a12..718a96258a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 4cc8fc9b37..7387a0aaf7 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -27,7 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/siyi/n7/default.px4board b/boards/siyi/n7/default.px4board index bb2fe317e9..2091a3ade4 100644 --- a/boards/siyi/n7/default.px4board +++ b/boards/siyi/n7/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/thepeach/k1/default.px4board b/boards/thepeach/k1/default.px4board index 4490263842..1ff45d4e24 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/thepeach/r1/default.px4board b/boards/thepeach/r1/default.px4board index 4490263842..1ff45d4e24 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -37,7 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_ROVER_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2ead22c570..70eb89b88b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - // differential_drive_control does allocation and publishes directly to actuator_motors topic + // rover_control does allocation and publishes directly to actuator_motors topic break; case EffectivenessSource::FIXED_WING: diff --git a/src/modules/differential_drive_control/CMakeLists.txt b/src/modules/rover_control/CMakeLists.txt similarity index 95% rename from src/modules/differential_drive_control/CMakeLists.txt rename to src/modules/rover_control/CMakeLists.txt index 3da82522fc..ad20bdfe7f 100644 --- a/src/modules/differential_drive_control/CMakeLists.txt +++ b/src/modules/rover_control/CMakeLists.txt @@ -34,11 +34,13 @@ add_subdirectory(DifferentialDriveKinematics) px4_add_module( - MODULE modules__differential_drive_control - MAIN differential_drive_control + MODULE modules__rover_control + MAIN rover_control SRCS DifferentialDriveControl.cpp DifferentialDriveControl.hpp + RoverControl.cpp + RoverControl.hpp DEPENDS DifferentialDriveKinematics px4_work_queue diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/rover_control/DifferentialDriveControl.cpp similarity index 90% rename from src/modules/differential_drive_control/DifferentialDriveControl.cpp rename to src/modules/rover_control/DifferentialDriveControl.cpp index 41e5093499..e7f21b1cb7 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.cpp +++ b/src/modules/rover_control/DifferentialDriveControl.cpp @@ -38,8 +38,8 @@ using namespace matrix; namespace differential_drive_control { -DifferentialDriveControl::DifferentialDriveControl() : - ModuleParams(nullptr), +DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : + ModuleParams(parent), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { updateParams(); @@ -47,7 +47,7 @@ DifferentialDriveControl::DifferentialDriveControl() : bool DifferentialDriveControl::init() { - ScheduleOnInterval(10_ms); // 100 Hz + // ScheduleOnInterval(10_ms); // 100 Hz return true; } @@ -128,27 +128,33 @@ void DifferentialDriveControl::Run() _actuator_motors_pub.publish(actuator_motors); } +void DifferentialDriveControl::Update() +{ + Run(); +} + int DifferentialDriveControl::task_spawn(int argc, char *argv[]) { - DifferentialDriveControl *instance = new DifferentialDriveControl(); + // DifferentialDriveControl *instance = new DifferentialDriveControl(); - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + // if (instance) { + // _object.store(instance); + // _task_id = task_id_is_work_queue; - if (instance->init()) { - return PX4_OK; - } + // if (instance->init()) { + // return PX4_OK; + // } - } else { - PX4_ERR("alloc failed"); - } + // } else { + // PX4_ERR("alloc failed"); + // } - delete instance; - _object.store(nullptr); - _task_id = -1; + // delete instance; + // _object.store(nullptr); + // _task_id = -1; - return PX4_ERROR; + // return PX4_ERROR; + return PX4_OK; } int DifferentialDriveControl::custom_command(int argc, char *argv[]) diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.hpp b/src/modules/rover_control/DifferentialDriveControl.hpp similarity index 98% rename from src/modules/differential_drive_control/DifferentialDriveControl.hpp rename to src/modules/rover_control/DifferentialDriveControl.hpp index 47133c8d27..aef17fc6b6 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.hpp +++ b/src/modules/rover_control/DifferentialDriveControl.hpp @@ -66,7 +66,7 @@ class DifferentialDriveControl : public ModuleBase, pu public px4::ScheduledWorkItem { public: - DifferentialDriveControl(); + DifferentialDriveControl(ModuleParams *parent); ~DifferentialDriveControl() override = default; /** @see ModuleBase */ @@ -80,6 +80,8 @@ public: bool init(); + void Update(); + protected: void updateParams() override; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/rover_control/DifferentialDriveKinematics/CMakeLists.txt similarity index 100% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt rename to src/modules/rover_control/DifferentialDriveKinematics/CMakeLists.txt diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp similarity index 100% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp rename to src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp similarity index 100% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp rename to src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp similarity index 100% rename from src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp rename to src/modules/rover_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp diff --git a/src/modules/differential_drive_control/Kconfig b/src/modules/rover_control/Kconfig similarity index 59% rename from src/modules/differential_drive_control/Kconfig rename to src/modules/rover_control/Kconfig index 9d4a0b5468..c333bc5924 100644 --- a/src/modules/differential_drive_control/Kconfig +++ b/src/modules/rover_control/Kconfig @@ -1,5 +1,5 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL - bool "differential_drive_control" +menuconfig MODULES_ROVER_CONTROL + bool "ROVER_CONTROL" default n depends on MODULES_CONTROL_ALLOCATOR ---help--- diff --git a/src/modules/rover_control/RoverControl.cpp b/src/modules/rover_control/RoverControl.cpp new file mode 100644 index 0000000000..f85da04b5c --- /dev/null +++ b/src/modules/rover_control/RoverControl.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "RoverControl.hpp" + +using namespace time_literals; +using namespace matrix; +namespace rover_control +{ + +RoverControl::RoverControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _differential_drive_control(this) +{ + updateParams(); +} + +bool RoverControl::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverControl::updateParams() +{ + ModuleParams::updateParams(); +} + +void RoverControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + _differential_drive_control.Update(); + +} + +int RoverControl::task_spawn(int argc, char *argv[]) +{ + RoverControl *instance = new RoverControl(); + + 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 RoverControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RoverControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover state machine. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_control_main(int argc, char *argv[]) +{ + return RoverControl::main(argc, argv); +} + +} // namespace rover_control diff --git a/src/modules/rover_control/RoverControl.hpp b/src/modules/rover_control/RoverControl.hpp new file mode 100644 index 0000000000..528232b242 --- /dev/null +++ b/src/modules/rover_control/RoverControl.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +// PX4 includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard library includes +#include + +// Local includes +#include +#include "DifferentialDriveControl.hpp" + +namespace rover_control +{ + +class RoverControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + RoverControl(); + ~RoverControl() override = default; + + /** @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(); + +protected: + void updateParams() override; + +private: + void Run() override; + + uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; + + // differential_drive_setpoint_s _differential_drive_setpoint{}; + // DifferentialDriveKinematics _differential_drive_kinematics{}; + differential_drive_control::DifferentialDriveControl _differential_drive_control; + + bool _armed = false; + bool _manual_driving = false; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_speed_scale, + (ParamFloat) _param_rdd_ang_velocity_scale, + (ParamFloat) _param_rdd_max_wheel_speed, + (ParamFloat) _param_rdd_wheel_base, + (ParamFloat) _param_rdd_wheel_radius, + (ParamInt) _param_r_rev + ) +}; + +} // namespace rover_control diff --git a/src/modules/differential_drive_control/module.yaml b/src/modules/rover_control/module.yaml similarity index 100% rename from src/modules/differential_drive_control/module.yaml rename to src/modules/rover_control/module.yaml diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceMotor.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp similarity index 100% rename from src/modules/simulation/gz_bridge/GZMixingInterfaceMotor.cpp rename to src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceMotor.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp similarity index 100% rename from src/modules/simulation/gz_bridge/GZMixingInterfaceMotor.hpp rename to src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp