From 3de5c609a4fde1410e18a1b3a4d00acccb644d4f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 4 Dec 2023 11:21:15 +0100 Subject: [PATCH] Differential Rover: PR fixes --- .../airframes/1060_gazebo-classic_rover | 2 +- .../airframes/1061_gazebo-classic_r1_rover | 22 --- .../init.d-posix/airframes/4009_gz_r1_rover | 41 ++--- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 2 - .../airframes/50000_generic_ground_vehicle | 2 +- .../airframes/50003_aion_robotics_r1_rover | 39 +--- .../init.d/rc.rover_ackermann_apps | 28 --- .../init.d/rc.rover_ackermann_defaults | 22 --- ROMFS/px4fmu_common/init.d/rc.rover_apps | 2 - .../init.d/rc.rover_differential_apps | 17 +- .../init.d/rc.rover_differential_defaults | 14 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 27 +-- boards/px4/fmu-v5x/default.px4board | 2 +- boards/px4/sitl/default.px4board | 2 +- .../include/px4_platform_common/module.h | 2 +- .../ros2/include/px4_platform_common/module.h | 2 +- .../control_allocator/ControlAllocator.cpp | 3 +- .../differential_drive_control/CMakeLists.txt | 5 +- .../DifferentialDriveControl.cpp | 168 ++++++++---------- .../DifferentialDriveControl.hpp | 51 +++--- .../CMakeLists.txt | 2 - .../DifferentialDriveKinematics.cpp | 44 +++-- .../DifferentialDriveKinematics.hpp | 36 ++-- .../DifferentialDriveKinematicsTest.cpp | 148 ++++++++++++--- .../differential_drive_control/Kconfig | 2 +- .../differential_drive_control/module.yaml | 55 ++++++ .../differential_drive_control/params.c | 84 --------- src/modules/logger/logged_topics.cpp | 2 +- .../simulation/gz_bridge/CMakeLists.txt | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 10 +- src/modules/simulation/gz_bridge/GZBridge.hpp | 2 +- .../gz_bridge/GZMixingInterfaceWheel.cpp | 5 +- .../gz_bridge/GZMixingInterfaceWheel.hpp | 2 +- src/modules/simulation/gz_bridge/module.yaml | 6 +- src/modules/uxrce_dds_client/dds_topics.yaml | 6 +- 35 files changed, 381 insertions(+), 478 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults delete mode 100644 src/modules/differential_drive_control/params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover index 97e389d47c..d58f5aebae 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover @@ -3,7 +3,7 @@ # @name Rover # -. ${R}etc/init.d/rc.rover_ackermann_defaults +. ${R}etc/init.d/rc.rover_defaults param set-default GND_L1_DIST 5 param set-default GND_L1_PERIOD 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover index b612b20f3d..54905d502b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover @@ -6,29 +6,7 @@ . ${R}etc/init.d/rc.rover_differential_defaults -param set-default GND_L1_DIST 5 -param set-default GND_SP_CTRL_MODE 1 -param set-default GND_SPEED_D 3 -param set-default GND_SPEED_I 0.001 -param set-default GND_SPEED_IMAX 0.125 -param set-default GND_SPEED_P 0.25 -param set-default GND_SPEED_THR_SC 1 -param set-default GND_SPEED_TRIM 4 -param set-default GND_THR_CRUISE 0.3 -param set-default GND_THR_MAX 0.5 -param set-default GND_THR_MIN 0 - -param set-default NAV_ACC_RAD 0.5 -param set-default NAV_LOITER_RAD 2 - -param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 - -param set-default CA_AIRFRAME 6 - -param set-default CA_R_REV 3 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 101 param set-default PWM_MAIN_FUNC6 102 param set-default PWM_MAIN_FUNC7 102 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index f6e51cea57..80664cf661 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -1,5 +1,4 @@ #!/bin/sh -# # @name Aion Robotics R1 Rover # @type Rover # @class Rover @@ -10,39 +9,23 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz} PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} -param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_EN 1 # Gazebo bridge +# Simulated sensors param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 -param set-default COM_PREARM_MODE 2 +# Actuator mapping +param set-default SIM_GZ_WH_FUNC1 101 # right wheel +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 -param set-default NAV_ACC_RAD 0.5 -param set-default NAV_LOITER_RAD 2 +param set-default SIM_GZ_WH_FUNC2 102 # left wheel +param set-default SIM_GZ_WH_MIN2 0 +param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_DIS2 100 -param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 - -param set-default CA_R_REV 3 - -param set-default SIM_GZ_EC_FUNC1 101 -param set-default SIM_GZ_EC_MIN1 0 -param set-default SIM_GZ_EC_MAX1 200 -param set-default SIM_GZ_EC_DIS1 100 - -param set-default SIM_GZ_EC_FUNC2 102 -param set-default SIM_GZ_EC_MIN2 0 -param set-default SIM_GZ_EC_MAX2 200 -param set-default SIM_GZ_EC_DIS2 100 - -param set-default SIM_GZ_MT_FUNC1 101 -param set-default SIM_GZ_MT_MIN1 0 -param set-default SIM_GZ_MT_MAX1 200 -param set-default SIM_GZ_MT_DIS1 100 - -param set-default SIM_GZ_MT_FUNC2 102 -param set-default SIM_GZ_MT_MIN2 0 -param set-default SIM_GZ_MT_MAX2 200 -param set-default SIM_GZ_MT_DIS2 100 +param set-default SIM_GZ_WH_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 883d3cfee1..4b45187ff5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -53,8 +53,6 @@ px4_add_romfs_files( rc.rover_defaults rc.rover_differential_apps rc.rover_differential_defaults - rc.rover_ackermann_apps - rc.rover_ackermann_defaults rc.uuv_apps rc.uuv_defaults rc.vehicle_setup diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle index 8191072a1a..ba4598ce83 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -13,7 +13,7 @@ # @board bitcraze_crazyflie exclude # -. ${R}etc/init.d/rc.rover_ackermann_defaults +. ${R}etc/init.d/rc.rover_defaults param set-default BAT1_N_CELLS 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index ac14124956..20a16428b2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -13,50 +13,13 @@ . ${R}etc/init.d/rc.rover_differential_defaults - - param set-default BAT1_N_CELLS 4 param set-default EKF2_GBIAS_INIT 0.01 param set-default EKF2_ANGERR_INIT 0.01 -param set-default EKF2_MAG_TYPE 1 - -param set-default FW_AIRSPD_MIN 0 -param set-default FW_AIRSPD_TRIM 1 -param set-default FW_AIRSPD_MAX 3 - -param set-default GND_SP_CTRL_MODE 1 -param set-default GND_L1_DIST 5 -param set-default GND_L1_PERIOD 3 -param set-default GND_THR_CRUISE 0.7 -param set-default GND_THR_MAX 0.5 - -# Because this is differential drive, it can make a turn with radius 0. -# This corresponds to a turn angle of pi radians. -# If a special case is made for differential-drive, this will need to change. -param set-default GND_MAX_ANG 3.142 -param set-default GND_WHEEL_BASE 0.3 - -# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase -# to support negative throttle. -param set-default GND_THR_MIN 0 -param set-default GND_SPEED_P 0.25 -param set-default GND_SPEED_I 3 -param set-default GND_SPEED_D 0.001 -param set-default GND_SPEED_IMAX 0.125 -param set-default GND_SPEED_THR_SC 1 - -param set-default NAV_ACC_RAD 0.5 - -# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians -param set-default GND_MAX_ANG 3.1415 # Set geometry & output configration -param set-default CA_AIRFRAME 6 -param set-default CA_R_REV 3 - - param set-default RBCLW_ADDRESS 128 param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 -param set-default RBCLW_REV 1 +param set-default RBCLW_REV 1 # reverse right wheels diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps deleted file mode 100644 index dc311e4ae7..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps +++ /dev/null @@ -1,28 +0,0 @@ -#!/bin/sh -# -# Standard apps for unmanned ground vehicles (UGV). -# -# NOTE: Script variables are declared/initialized/unset in the rcS script. -# - -# -# Start the attitude and position estimator. -# -ekf2 start & -#attitude_estimator_q start -#local_position_estimator start - -# -# Start Control Allocator -# -control_allocator start - -# -# Start attitude controllers. -# -rover_pos_control start - -# -# Start Land Detector. -# -land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults deleted file mode 100644 index 75f608082e..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults +++ /dev/null @@ -1,22 +0,0 @@ -#!/bin/sh -# -# UGV default parameters. -# -# NOTE: Script variables are declared/initialized/unset in the rcS script. -# - -set VEHICLE_TYPE rover_ackermann - -# MAV_TYPE_GROUND_ROVER 10 -param set-default MAV_TYPE 10 - -# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor -param set-default CBRK_AIRSPD_CHK 162128 - -param set-default MIS_TAKEOFF_ALT 0.01 - -param set-default NAV_ACC_RAD 2 -param set-default NAV_LOITER_RAD 2 - -# Temporary. -param set-default NAV_FW_ALT_RAD 1000 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index dc311e4ae7..1db046ace4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -9,8 +9,6 @@ # Start the attitude and position estimator. # ekf2 start & -#attitude_estimator_q start -#local_position_estimator start # # Start Control Allocator diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index c9aca4c310..31c79ac980 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -1,24 +1,11 @@ #!/bin/sh -# -# Standard apps for unmanned ground vehicles (UGV). -# -# NOTE: Script variables are declared/initialized/unset in the rcS script. -# +# Standard apps for a differential drive rover. -# # Start the attitude and position estimator. -# ekf2 start & -#attitude_estimator_q start -#local_position_estimator start -# -# Start manual rover differential drive controller. -# +# Start rover differential drive controller. differential_drive_control start - -# # Start Land Detector. -# land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index d818ad9a60..e0cace3a32 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -1,11 +1,11 @@ #!/bin/sh -# -# UGV default parameters. -# -# NOTE: Script variables are declared/initialized/unset in the rcS script. -# +# Differential rover parameters. set VEHICLE_TYPE rover_differential -# MAV_TYPE_GROUND_ROVER 10 -param set-default MAV_TYPE 10 +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER + +param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_R_REV 3 # Right and left motors reversible + +param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 4d02fb4d72..1b7ddee5a5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -23,24 +23,6 @@ then . ${R}etc/init.d/rc.mc_apps fi -# -# Differential Rover setup. -# -if [ $VEHICLE_TYPE = rover_differential ] -then - # Start standard UGV apps. - . ${R}etc/init.d/rc.rover_differential_apps -fi - -# -# Ackermann Rover setup. -# -if [ $VEHICLE_TYPE = rover_ackermann ] -then - # Start standard UGV apps. - . ${R}etc/init.d/rc.rover_ackermann_apps -fi - # # UGV setup. # @@ -50,6 +32,15 @@ then . ${R}etc/init.d/rc.rover_apps fi +# +# Differential Rover setup. +# +if [ $VEHICLE_TYPE = rover_differential ] +then + # Start differential drive rover apps. + . ${R}etc/init.d/rc.rover_differential_apps +fi + # # VTOL setup. # diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 7b542f5a3a..877e2d2e25 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -7,7 +7,6 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" -CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y @@ -54,6 +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_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 3a40e02a2a..008dc97a12 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_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y +CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y diff --git a/platforms/common/include/px4_platform_common/module.h b/platforms/common/include/px4_platform_common/module.h index 63867df4e2..13e5a5ccae 100644 --- a/platforms/common/include/px4_platform_common/module.h +++ b/platforms/common/include/px4_platform_common/module.h @@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex; * static int custom_command(int argc, char *argv[]) * { * // support for custom commands - * // it none are supported, just do: + * // if none are supported, just do: * return print_usage("unrecognized command"); * } * diff --git a/platforms/ros2/include/px4_platform_common/module.h b/platforms/ros2/include/px4_platform_common/module.h index 436c86ed92..bc8516806b 100644 --- a/platforms/ros2/include/px4_platform_common/module.h +++ b/platforms/ros2/include/px4_platform_common/module.h @@ -75,7 +75,7 @@ * static int custom_command(int argc, char *argv[]) * { * // support for custom commands - * // it none are supported, just do: + * // if none are supported, just do: * return print_usage("unrecognized command"); * } * diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a7b2af8998..2ead22c570 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -61,7 +61,6 @@ ControlAllocator::ControlAllocator() : _actuator_servos_pub.advertise(); _actuator_servos_trim_pub.advertise(); - for (int i = 0; i < MAX_NUM_MOTORS; ++i) { char buffer[17]; snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); @@ -240,7 +239,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - // actuator_motors message is published directly from the differential drive controller + // differential_drive_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/differential_drive_control/CMakeLists.txt index 7fd6f1d8f3..3da82522fc 100644 --- a/src/modules/differential_drive_control/CMakeLists.txt +++ b/src/modules/differential_drive_control/CMakeLists.txt @@ -30,17 +30,18 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + add_subdirectory(DifferentialDriveKinematics) px4_add_module( MODULE modules__differential_drive_control MAIN differential_drive_control - COMPILE_FLAGS - #-DDEBUG_BUILD SRCS DifferentialDriveControl.cpp DifferentialDriveControl.hpp DEPENDS DifferentialDriveKinematics px4_work_queue + MODULE_CONFIG + module.yaml ) diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/differential_drive_control/DifferentialDriveControl.cpp index a8b2ede499..41e5093499 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.cpp +++ b/src/modules/differential_drive_control/DifferentialDriveControl.cpp @@ -34,7 +34,7 @@ #include "DifferentialDriveControl.hpp" using namespace time_literals; - +using namespace matrix; namespace differential_drive_control { @@ -42,136 +42,118 @@ DifferentialDriveControl::DifferentialDriveControl() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _outputs_pub.advertise(); - _last_timestamp = hrt_absolute_time(); - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - _differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get()); + updateParams(); } -int DifferentialDriveControl::task_spawn(int argc, char *argv[]) -{ - DifferentialDriveControl *obj = new DifferentialDriveControl(); - - if (!obj) { - PX4_ERR("alloc failed"); - return -1; - } - - _object.store(obj); - _task_id = task_id_is_work_queue; - - obj->start(); - - return 0; -} - -void DifferentialDriveControl::start() +bool DifferentialDriveControl::init() { ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void DifferentialDriveControl::updateParams() +{ + ModuleParams::updateParams(); + _max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get(); + _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); + + _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + _differential_drive_kinematics.setMaxSpeed(_max_speed); + _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); } void DifferentialDriveControl::Run() { - if (should_exit()) { ScheduleClear(); exit_and_cleanup(); } - _current_timestamp = hrt_absolute_time(); - - vehicle_control_mode_poll(); + hrt_abstime now = hrt_absolute_time(); if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); updateParams(); - - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - _differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get()); } - if (_vehicle_control_mode.flag_armed) { + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode; + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported + } + } + + if (_manual_driving) { + // Manual mode + // directly produce setpoints from the manual control setpoint (joystick) if (_manual_control_setpoint_sub.updated()) { manual_control_setpoint_s manual_control_setpoint{}; - _manual_control_setpoint_sub.copy(&manual_control_setpoint); - - // Manual mode - if (_vehicle_control_mode.flag_control_manual_enabled) { - // directly get the input from the manual control setpoint (joystick) - - _differential_drive_setpoint.timestamp = hrt_absolute_time(); - - _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_max_speed.get(); - _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_max_angular_velocity.get(); + if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + _differential_drive_setpoint.timestamp = now; + _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed; + _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * + _max_angular_velocity; _differential_drive_setpoint_pub.publish(_differential_drive_setpoint); } } + } - // if manual is not enabled, it will look for a differential drive setpoint topic (differential_drive_setpoint) and subscribe to it from a ROS2 mode over DDS - if (_differential_drive_setpoint_sub.updated()) { - _differential_drive_setpoint_sub.copy(&_differential_drive_setpoint); + _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - _velocity_control_inputs(0) = _differential_drive_setpoint.speed; - _velocity_control_inputs(1) = _differential_drive_setpoint.yaw_rate; + // publish data to actuator_motors (output module) + // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) + Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics( + _differential_drive_setpoint.speed, + _differential_drive_setpoint.yaw_rate); - } else if (_differential_drive_setpoint.timestamp <= 100_ms) { - _velocity_control_inputs(0) = 0.0f; - _velocity_control_inputs(1) = 0.0f; + // Check if max_angular_wheel_speed is zero + const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now; + const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON; + + if (!_armed || setpoint_timeout || !valid_max_speed) { + wheel_speeds = {}; // stop + } + + wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); + + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults + wheel_speeds.copyTo(actuator_motors.control); + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); +} + +int DifferentialDriveControl::task_spawn(int argc, char *argv[]) +{ + DifferentialDriveControl *instance = new DifferentialDriveControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; } } else { - // if the system is in an error state, stop the vehicle - _velocity_control_inputs = {0.0f, 0.0f}; + PX4_ERR("alloc failed"); } - // publish data to actuator_motors (output module) - publishWheelControl(); - - _last_timestamp = _current_timestamp; + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; } -void DifferentialDriveControl::publishWheelControl() +int DifferentialDriveControl::custom_command(int argc, char *argv[]) { - // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) - _output_inverse = _differential_drive_kinematics.computeInverseKinematics(_velocity_control_inputs(0), - _velocity_control_inputs(1)); - - // Superpose Linear and Angular velocity vector - float max_angular_wheel_speed = ((_param_rdd_max_speed.get() + (_param_rdd_max_angular_velocity.get() * - _param_rdd_wheel_base.get() / 2.f)) / _param_rdd_wheel_radius.get()); - - // Check if max_angular_wheel_speed is zero - if (fabsf(max_angular_wheel_speed) < FLT_EPSILON) { - // Guard against division by zero - max_angular_wheel_speed = 0.001f; - } - - _actuator_motors.timestamp = hrt_absolute_time(); - - // bitset which motors are configured to be reversible (3 -> ..00 0000 0011 this means that the first two motors are reversible) - _actuator_motors.reversible_flags = 3; - _actuator_motors.control[0] = _output_inverse(0) / max_angular_wheel_speed; - _actuator_motors.control[1] = _output_inverse(1) / max_angular_wheel_speed; - - _outputs_pub.publish(_actuator_motors); -} - -void DifferentialDriveControl::vehicle_control_mode_poll() -{ - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } -} - -int DifferentialDriveControl::print_status() -{ - PX4_INFO("Diffential Drive Controller running"); - return 0; + return print_usage("unknown command"); } int DifferentialDriveControl::print_usage(const char *reason) @@ -186,8 +168,8 @@ int DifferentialDriveControl::print_usage(const char *reason) Rover Differential Drive controller. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("differential_drive", "system"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.hpp b/src/modules/differential_drive_control/DifferentialDriveControl.hpp index 9364bd6e82..47133c8d27 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.hpp +++ b/src/modules/differential_drive_control/DifferentialDriveControl.hpp @@ -51,9 +51,6 @@ #include #include #include - -// Cruise mode -#include #include // Standard library includes @@ -72,55 +69,47 @@ public: DifferentialDriveControl(); ~DifferentialDriveControl() override = default; + /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]) - { - return print_usage("unknown command"); - } + static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::print_status() */ - int print_status() override; + bool init(); - void start(); +protected: + void updateParams() override; private: - void Run() override; - void publishWheelControl(); - void vehicle_control_mode_poll(); - uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_motors)}; - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - - - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + 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::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + + 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{}; - vehicle_control_mode_s _vehicle_control_mode{}; - actuator_motors_s _actuator_motors{}; + DifferentialDriveKinematics _differential_drive_kinematics{}; - DifferentialDriveKinematics _differential_drive_kinematics; - - matrix::Vector2f _velocity_control_inputs{0.f, 0.f}; // [m/s] collective roll-off speed in body x-axis, [rad/s] yaw rate - matrix::Vector2f _output_inverse{0.0f, 0.0f}; // [rad/s] Right Motor, [rad/s] Left Motor - - float _last_timestamp{0.f}; - float _current_timestamp{0.f}; + bool _armed = false; + bool _manual_driving = false; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_max_speed, - (ParamFloat) _param_rdd_max_angular_velocity, + (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 + (ParamFloat) _param_rdd_wheel_radius, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt index 2ba1fc7b86..c352d7c597 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt @@ -33,10 +33,8 @@ px4_add_library(DifferentialDriveKinematics DifferentialDriveKinematics.cpp - DifferentialDriveKinematics.hpp ) -target_compile_options(DifferentialDriveKinematics PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp index 2e1cd6a3b7..ffa96cbe90 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp @@ -33,20 +33,36 @@ #include "DifferentialDriveKinematics.hpp" -void DifferentialDriveKinematics::setWheelBase(float wheel_base) +#include + +using namespace matrix; + +matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) { - _wheel_base = wheel_base; + if (_max_speed < FLT_EPSILON) { + return Vector2f(); + } + + linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); + yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); + + const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate; + float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity); + + // Compute an initial gain + float gain = 1.0f; + + if (combined_velocity > _max_speed) { + float excess_velocity = fabsf(combined_velocity - _max_speed); + float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; + gain = adjusted_linear_velocity / fabsf(linear_velocity_x); + } + + // Apply the gain + linear_velocity_x *= gain; + + // Calculate the left and right wheel speeds + return Vector2f(linear_velocity_x - rotational_velocity, + linear_velocity_x + rotational_velocity) / _max_speed; } -void DifferentialDriveKinematics::setWheelRadius(float wheel_radius) -{ - _wheel_radius = wheel_radius; -} - -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_vel_x, float yaw_rate) -{ - float motor_vel_right = linear_vel_x / _wheel_radius - _wheel_base / 2.f * yaw_rate / _wheel_radius; - float motor_vel_left = linear_vel_x / _wheel_radius + _wheel_base / 2.f * yaw_rate / _wheel_radius; - - return matrix::Vector2f(motor_vel_right, motor_vel_left); -} diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp index f25c18ee66..193a7d05fb 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp @@ -47,30 +47,38 @@ public: DifferentialDriveKinematics() = default; ~DifferentialDriveKinematics() = default; - /** - * @brief Computes the inverse kinematics for differential drive. - * - * @param linear_vel_x Linear velocity along the x-axis. - * @param yaw_rate Yaw rate of the robot. - * @return matrix::Vector2f Motor velocities for the right and left motors. - */ - matrix::Vector2f computeInverseKinematics(float linear_vel_x, float yaw_rate); - /** * @brief Sets the wheel base of the robot. * * @param wheel_base The distance between the centers of the wheels. */ - void setWheelBase(float wheel_base); + void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; /** - * @brief Sets the radius of the wheels of the robot. + * @brief Sets the maximum speed of the robot. * - * @param wheel_radius The radius of the wheels. + * @param max_speed The maximum speed of the robot. */ - void setWheelRadius(float wheel_radius); + void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; + + /** + * @brief Sets the maximum angular speed of the robot. + * + * @param max_angular_speed The maximum angular speed of the robot. + */ + void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; + + /** + * @brief Computes the inverse kinematics for differential drive. + * + * @param linear_velocity_x Linear velocity along the x-axis. + * @param yaw_rate Yaw rate of the robot. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate); private: float _wheel_base{0.f}; - float _wheel_radius{0.f}; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; }; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp index 1b0e514938..6eee43c41e 100644 --- a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp @@ -37,48 +37,138 @@ using namespace matrix; -TEST(DifferentialDriveKinematicsTest, AllZeroCaseInverse) +TEST(DifferentialDriveKinematicsTest, AllZeroInputCase) { DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); - kinematics.setWheelRadius(1.f); - Vector2f rate_setpoint = {0.f, 0.f}; - kinematics.setInput(rate_setpoint, true); - Vector2f wheel_output = kinematics.getOutput(true); - EXPECT_EQ(wheel_output, Vector2f()); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with zero linear velocity and zero yaw rate (stationary vehicle) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); } -TEST(DifferentialDriveKinematicsTest, InvalidCaseInverse) + +TEST(DifferentialDriveKinematicsTest, InvalidParameterCase) { DifferentialDriveKinematics kinematics; kinematics.setWheelBase(0.f); - kinematics.setWheelRadius(0.f); - Vector2f rate_setpoint = {0.f, 0.f}; - kinematics.setInput(rate_setpoint, true); - Vector2f wheel_output = kinematics.getOutput(true); - EXPECT_EQ(wheel_output, Vector2f()); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with invalid parameters (zero wheel base and wheel radius) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); } -TEST(DifferentialDriveKinematicsTest, UnitCaseInverse) -{ - DifferentialDriveKinematics kinematics; - kinematics.setWheelBase(1.f); - kinematics.setWheelRadius(1.f); - Vector2f rate_setpoint = {1.f, 1.f}; - kinematics.setInput(rate_setpoint, true); - Vector2f wheel_output = kinematics.getOutput(true); - Vector2f expected_output = {1.5f, 0.5f}; - EXPECT_EQ(wheel_output, expected_output); -} TEST(DifferentialDriveKinematicsTest, UnitCase) { DifferentialDriveKinematics kinematics; kinematics.setWheelBase(1.f); - kinematics.setWheelRadius(1.f); - Vector2f wheel_input = {1.f, 1.f}; - kinematics.setInput(wheel_input, false); - Vector2f rate_output = kinematics.getOutput(false); - Vector2f expected_output = {1.f, 0.f}; - EXPECT_EQ(rate_output, expected_output); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with unit values for linear velocity and yaw rate + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); +} + + +TEST(DifferentialDriveKinematicsTest, UnitSaturationCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); +} + + +TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); +} + +TEST(DifferentialDriveKinematicsTest, RandomCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); +} + +TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test rotating in place (zero linear velocity, non-zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); +} + +TEST(DifferentialDriveKinematicsTest, StraightMovementCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test moving straight (non-zero linear velocity, zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MinInputValuesCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(FLT_MIN); + kinematics.setMaxSpeed(FLT_MIN); + kinematics.setMaxAngularVelocity(FLT_MIN); + + // Test with minimum possible input values + EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST(DifferentialDriveKinematicsTest, MaxAngularCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); } diff --git a/src/modules/differential_drive_control/Kconfig b/src/modules/differential_drive_control/Kconfig index 5fb5bbf319..559f65969e 100644 --- a/src/modules/differential_drive_control/Kconfig +++ b/src/modules/differential_drive_control/Kconfig @@ -2,4 +2,4 @@ menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL bool "differential_drive_control" default y ---help--- - Enable support for differential_drive_control + Enable support for control of differential drive rovers diff --git a/src/modules/differential_drive_control/module.yaml b/src/modules/differential_drive_control/module.yaml index e69de29bb2..77bd61dda4 100644 --- a/src/modules/differential_drive_control/module.yaml +++ b/src/modules/differential_drive_control/module.yaml @@ -0,0 +1,55 @@ +module_name: Differential Drive Control + +parameters: + - group: Rover Differential Drive + definitions: + RDD_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the center of the right wheel to the center of the left wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + RDD_WHEEL_RADIUS: + description: + short: Wheel radius + long: Size of the wheel, half the diameter of the wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.1 + RDD_SPEED_SCALE: + description: + short: Manual speed scale + type: float + min: 0 + max: 1.0 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_ANG_SCALE: + description: + short: Manual angular velocity scale + type: float + min: 0 + max: 1.0 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_WHL_SPEED: + description: + short: Maximum wheel speed + type: float + unit: rad/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 10 diff --git a/src/modules/differential_drive_control/params.c b/src/modules/differential_drive_control/params.c deleted file mode 100644 index 670711a996..0000000000 --- a/src/modules/differential_drive_control/params.c +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * Wheel Base - * - * Distance from the center of the right wheel to the center of the left wheel - * - * @unit m - * @min 0.001 - * @max 100 - * @increment 0.001 - * @decimal 3 - * @group Rover Differential Drive - */ -PARAM_DEFINE_FLOAT(RDD_WHEEL_BASE, 0.5f); - -/** - * Wheel radius - * - * Size of the wheel, half the diameter of the wheel - * - * @unit m - * @min 0.001 - * @max 100 - * @increment 0.001 - * @decimal 3 - * @group Rover Differential Drive - */ -PARAM_DEFINE_FLOAT(RDD_WHEEL_RADIUS, 0.1f); - -/** - * Max Speed - * - * @unit m/s - * @min 0.0 - * @max 100 - * @increment 0.01 - * @decimal 2 - * @group Rover Differential Drive - */ -PARAM_DEFINE_FLOAT(RDD_MAX_SPEED, 0.5f); - -/** - * Max Angular Velocity - * - * @unit rad/s - * @min 0.0 - * @max 100 - * @increment 0.01 - * @decimal 2 - * @group Rover Differential Drive - */ -PARAM_DEFINE_FLOAT(RDD_MAX_ANG_VEL, 0.3f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d094077501..9665ca8f61 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -57,7 +57,7 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_topic("differential_drive_setpoint", 100); + add_optional_topic("differential_drive_setpoint", 100); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 6a39c06fab..ba36f448d3 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# Copyright (c) 2022-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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index b9e5c81a60..eda32df25c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -226,7 +226,7 @@ int GZBridge::init() return PX4_ERROR; } - if (!_mixing_interface_motor.init(_model_name)) { + if (!_mixing_interface_wheel.init(_model_name)) { PX4_ERR("failed to init motor output"); return PX4_ERROR; } @@ -729,7 +729,7 @@ void GZBridge::Run() _mixing_interface_esc.stop(); _mixing_interface_servo.stop(); - _mixing_interface_motor.stop(); + _mixing_interface_wheel.stop(); exit_and_cleanup(); return; @@ -745,7 +745,7 @@ void GZBridge::Run() _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); - _mixing_interface_motor.updateParams(); + _mixing_interface_wheel.updateParams(); } ScheduleDelayed(10_ms); @@ -761,8 +761,8 @@ int GZBridge::print_status() PX4_INFO_RAW("Servo outputs:\n"); _mixing_interface_servo.mixingOutput().printStatus(); - PX4_INFO_RAW("Motor outputs:\n"); - _mixing_interface_motor.mixingOutput().printStatus(); + PX4_INFO_RAW("Wheel outputs:\n"); + _mixing_interface_wheel.mixingOutput().printStatus(); return 0; } diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8c7a128d3c..8bf7ab06c0 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -132,7 +132,7 @@ private: GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; - GZMixingInterfaceWheel _mixing_interface_motor{_node, _node_mutex}; + GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; px4::atomic _world_time_us{0}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index e072c4c921..a80286db62 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -77,8 +77,9 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0); for (unsigned i = 0; i < active_output_count; i++) { - float output_scaler = 100.0f; - float scaled_output = (float)outputs[i] - output_scaler; + // Offsetting the output allows for negative values despite unsigned integer to reverse the wheels + static constexpr float output_offset = 100.0f; + float scaled_output = (float)outputs[i] - output_offset; wheel_velocity_message.set_velocity(i, scaled_output); } diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index bf36644302..281606dd61 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -80,7 +80,7 @@ private: gz::transport::Node &_node; pthread_mutex_t &_node_mutex; - MixingOutput _mixing_output{"SIM_GZ_MT", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; gz::transport::Node::Publisher _actuators_pub; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 81b382945c..35cccb61bc 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -25,9 +25,9 @@ actuator_output: max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } num_channels: 8 - - param_prefix: SIM_GZ_MT - group_label: 'Motors' - channel_label: 'Motors' + - param_prefix: SIM_GZ_WH + group_label: 'Wheels' + channel_label: 'Wheels' standard_params: disarmed: { min: 0, max: 200, default: 100 } min: { min: 0, max: 200, default: 0 } diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index ed914bdabb..255c8e0b54 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -76,9 +76,6 @@ subscriptions: - topic: /fmu/in/register_ext_component_request type: px4_msgs::msg::RegisterExtComponentRequest - - topic: /fmu/in/differential_drive_setpoint - type: px4_msgs::msg::DifferentialDriveSetpoint - - topic: /fmu/in/unregister_ext_component type: px4_msgs::msg::UnregisterExtComponent @@ -130,6 +127,9 @@ subscriptions: - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint + - topic: /fmu/in/differential_drive_setpoint + type: px4_msgs::msg::DifferentialDriveSetpoint + - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry