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 80664cf661..7697e23db7 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 @@ -6,16 +6,36 @@ . ${R}etc/init.d/rc.rover_differential_defaults PX4_SIMULATOR=${PX4_SIMULATOR:=gz} -PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge +# Rover parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 6 +param set-default RD_MAX_JERK 30 +param set-default RD_MAX_SPEED 7 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 180 +param set-default RD_MISS_SPD_DEF 7 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 + # 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 SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 1dcb917a09..01fc515c43 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -20,9 +20,9 @@ param set-default SENS_EN_ARSPDSIM 1 param set-default COM_ARM_WO_GPS 1 # Set Differential Drive Kinematics Library parameters: -param set RDD_WHEEL_BASE 0.9 -param set RDD_WHEEL_RADIUS 0.22 -param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h +param set RD_WHEEL_BASE 0.9 +param set RD_WHEEL_RADIUS 0.22 +param set RD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h # Actuator mapping - set SITL motors/servos output parameters: @@ -41,9 +41,9 @@ param set-default SIM_GZ_WH_FUNC2 102 # left wheel param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels -# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. -# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate -# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator +# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. +# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate +# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 24dea3fcda..3319779096 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -78,7 +78,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( rc.rover_differential_apps rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f763a43766..c6bae3566a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -143,7 +143,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( 50003_aion_robotics_r1_rover ) diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 275664af7e..4acbe83014 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -2,7 +2,7 @@ # Standard apps for a differential drive rover. # Start rover differential drive controller. -differential_drive start +rover_differential 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 e0cace3a32..4cddd44f89 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -3,9 +3,8 @@ set VEHICLE_TYPE rover_differential -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 +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 NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 92c265280a..07bfbe0941 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -17,5 +17,6 @@ CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 46d04d8cc2..a48b095687 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -54,7 +54,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 738e6d9b0d..4a4458c2e6 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -14,4 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 553bc76a52..7ded4008d8 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 553bc76a52..7ded4008d8 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 553bc76a52..7ded4008d8 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 1015558424..95a0155226 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -14,5 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index d46a7cc7e7..784be31db6 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y @@ -47,6 +46,7 @@ CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 65ec54d8b0..f0f6d520c7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -71,7 +71,6 @@ set(msg_files DebugKeyValue.msg DebugValue.msg DebugVect.msg - DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg @@ -181,6 +180,8 @@ set(msg_files RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg RoverAckermannStatus.msg + RoverDifferentialGuidanceStatus.msg + RoverDifferentialStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg deleted file mode 100644 index f7e4c58400..0000000000 --- a/msg/DifferentialDriveSetpoint.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 speed # [m/s] collective roll-off speed in body x-axis -bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward -float32 yaw_rate # [rad/s] yaw rate -bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward - -# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg new file mode 100644 index 0000000000..836546e7eb --- /dev/null +++ b/msg/RoverDifferentialGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 desired_speed # [m/s] Desired forward speed for the rover +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error_deg # [deg] Heading error of the pure pursuit controller +float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions +float32 pid_throttle_integral # Integral of the PID for the throttle during missions +uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] + +# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg new file mode 100644 index 0000000000..31907ffa64 --- /dev/null +++ b/msg/RoverDifferentialStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Actual forward speed of the rover +float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate +float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller + +# TOPICS rover_differential_status diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3399bd6190..48363eeb1e 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_differential_control does allocation and publishes directly to actuator_motors topic break; case EffectivenessSource::FIXED_WING: diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp deleted file mode 100644 index a1d34e01e7..0000000000 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDrive.hpp" - -DifferentialDrive::DifferentialDrive() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) -{ - updateParams(); -} - -bool DifferentialDrive::init() -{ - ScheduleOnInterval(10_ms); // 100 Hz - return true; -} - -void DifferentialDrive::updateParams() -{ - ModuleParams::updateParams(); - - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - - _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _differential_drive_guidance.setMaxSpeed(_max_speed); - _differential_drive_kinematics.setMaxSpeed(_max_speed); - - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); - _differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); - _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); -} - -void DifferentialDrive::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; - _time_stamp_last = now; - - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - } - - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode{}; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; - _mission_driving = vehicle_control_mode.flag_control_auto_enabled; - } - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); - _differential_drive_kinematics.setArmed(spooled_up); - _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); - } - } - - 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{}; - - if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - differential_drive_setpoint_s setpoint{}; - setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; - setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; - - // if acro mode, we activate the yaw rate control - if (_acro_driving) { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = true; - - } else { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = false; - } - - setpoint.timestamp = now; - _differential_drive_setpoint_pub.publish(setpoint); - } - } - - } else if (_mission_driving) { - // Mission mode - // directly receive setpoints from the guidance library - _differential_drive_guidance.computeGuidance( - _differential_drive_control.getVehicleYaw(), - _differential_drive_control.getVehicleBodyYawRate(), - dt - ); - } - - _differential_drive_control.control(dt); - _differential_drive_kinematics.allocate(); -} - -int DifferentialDrive::task_spawn(int argc, char *argv[]) -{ - DifferentialDrive *instance = new DifferentialDrive(); - - 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 DifferentialDrive::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int DifferentialDrive::print_usage(const char *reason) -{ - if (reason) { - PX4_ERR("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Rover Differential Drive controller. -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("differential_drive", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - return 0; -} - -extern "C" __EXPORT int differential_drive_main(int argc, char *argv[]) -{ - return DifferentialDrive::main(argc, argv); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt deleted file mode 100644 index 4610fa6c3d..0000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2024 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(DifferentialDriveControl - DifferentialDriveControl.cpp -) - -target_link_libraries(DifferentialDriveControl PUBLIC pid) -target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp deleted file mode 100644 index 728c5e666a..0000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDriveControl.hpp" - -using namespace matrix; - -DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) -{ - pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveControl::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_pid_angular_velocity, - _param_rdd_p_gain_angular_velocity.get(), // Proportional gain - _param_rdd_i_gain_angular_velocity.get(), // Integral gain - 0, // Derivative gain - 20, // Integral limit - 200); // Output limit - - pid_set_parameters(&_pid_speed, - _param_rdd_p_gain_speed.get(), // Proportional gain - _param_rdd_i_gain_speed.get(), // Integral gain - 0, // Derivative gain - 2, // Integral limit - 200); // Output limit -} - -void DifferentialDriveControl::control(float dt) -{ - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - - if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; - } - } - - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - - if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - _vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); - } - } - - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - - if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = velocity_in_body_frame(0); - } - } - - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - - // PID to reach setpoint using control_output - differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; - - if (_differential_drive_setpoint.closed_loop_speed_control) { - differential_drive_control_output.speed += - pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt); - } - - if (_differential_drive_setpoint.closed_loop_yaw_rate_control) { - differential_drive_control_output.yaw_rate += - pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); - } - - differential_drive_control_output.timestamp = hrt_absolute_time(); - _differential_drive_control_output_pub.publish(differential_drive_control_output); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp deleted file mode 100644 index 8d3b7e1c21..0000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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. - * - ****************************************************************************/ - -/** - * @file DifferentialDriveControl.hpp - * - * Controller for heading rate and forward speed. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class DifferentialDriveControl : public ModuleParams -{ -public: - DifferentialDriveControl(ModuleParams *parent); - ~DifferentialDriveControl() = default; - - void control(float dt); - float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } - float getVehicleYaw() const { return _vehicle_yaw; } - -protected: - void updateParams() override; - -private: - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - - uORB::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; - - differential_drive_setpoint_s _differential_drive_setpoint{}; - - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw{0.f}; - - // States - float _vehicle_body_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - - PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. - PID_t _pid_speed; ///< The PID controller for velocity. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_speed, - (ParamFloat) _param_rdd_i_gain_speed, - (ParamFloat) _param_rdd_p_gain_angular_velocity, - (ParamFloat) _param_rdd_i_gain_angular_velocity - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp deleted file mode 100644 index df453ff8b5..0000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDriveGuidance.hpp" - -#include - -using namespace matrix; - -DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - - pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) -{ - if (_position_setpoint_triplet_sub.updated()) { - _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); - } - - if (_vehicle_global_position_sub.updated()) { - _vehicle_global_position_sub.copy(&_vehicle_global_position); - } - - matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); - matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); - matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); - - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), - current_waypoint(0), - current_waypoint(1)); - - float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), - current_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); - - if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::TURNING; - } - - // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. - if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { - _currentState = GuidanceState::GOAL_REACHED; - } - - float desired_speed = 0.f; - - switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; - - if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; - } - - break; - - case GuidanceState::DRIVING: { - const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), - _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); - _forwards_velocity_smoothing.updateDurations(max_velocity); - _forwards_velocity_smoothing.updateTraj(dt); - desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, - _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); - break; - } - - case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle - desired_speed = 0.f; - heading_error = 0.f; - angular_velocity = 0.f; - _desired_angular_velocity = 0.f; - break; - } - - // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. - float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, - dt) + heading_error; - - differential_drive_setpoint_s output{}; - output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); - output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); - output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; - output.timestamp = hrt_absolute_time(); - - _differential_drive_setpoint_pub.publish(output); - - _current_waypoint = current_waypoint; -} - -void DifferentialDriveGuidance::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_heading_p_controller, - _param_rdd_p_gain_heading.get(), // Proportional gain - 0, // Integral gain - 0, // Derivative gain - 0, // Integral limit - 200); // Output limit - - _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); - _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); - _forwards_velocity_smoothing.setMaxVel(_max_speed); -} diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp deleted file mode 100644 index 0a5a29bbca..0000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. -}; - -/** - * @brief Class for differential drive guidance. - */ -class DifferentialDriveGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for DifferentialDriveGuidance. - * @param parent The parent ModuleParams object. - */ - DifferentialDriveGuidance(ModuleParams *parent); - ~DifferentialDriveGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param global_pos The global position of the vehicle in degrees. - * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. - * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. - * @param vehicle_yaw The yaw orientation of the vehicle in radians. - * @param body_velocity The velocity of the vehicle in m/s. - * @param angular_velocity The angular velocity of the vehicle in rad/s. - * @param dt The time step in seconds. - */ - void computeGuidance(float yaw, float angular_velocity, float dt); - - /** - * @brief Set the maximum speed for the vehicle. - * @param max_speed The maximum speed in m/s. - * @return The set maximum speed in m/s. - */ - float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } - - - /** - * @brief Set the maximum angular velocity for the vehicle. - * @param max_angular_velocity The maximum angular velocity in rad/s. - * @return The set maximum angular velocity in rad/s. - */ - float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - - position_setpoint_triplet_s _position_setpoint_triplet{}; - vehicle_global_position_s _vehicle_global_position{}; - - GuidanceState _currentState; ///< The current state of guidance. - - float _desired_angular_velocity; ///< The desired angular velocity. - - float _max_speed; ///< The maximum speed. - float _max_angular_velocity; ///< The maximum angular velocity. - - matrix::Vector2d _current_waypoint; ///< The current waypoint. - - VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. - PositionSmoothing _position_smoothing; ///< The position smoothing. - - PID_t _heading_p_controller; ///< The PID controller for yaw rate. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_heading, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt deleted file mode 100644 index c556db2b8f..0000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(DifferentialDriveKinematics - DifferentialDriveKinematics.cpp -) - -target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp deleted file mode 100644 index 42fcbea56b..0000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 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 "DifferentialDriveKinematics.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent) -{} - -void DifferentialDriveKinematics::allocate() -{ - hrt_abstime now = hrt_absolute_time(); - - if (_differential_drive_control_output_sub.updated()) { - _differential_drive_control_output_sub.copy(&_differential_drive_control_output); - } - - const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now; - - Vector2f wheel_speeds = - computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate); - - if (!_armed || setpoint_timeout) { - 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); -} - -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const -{ - 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); - const 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; -} diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp deleted file mode 100644 index 524a5520a7..0000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include - -/** - * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. - * - * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics - * given linear velocity and yaw rate. - */ -class DifferentialDriveKinematics : public ModuleParams -{ -public: - DifferentialDriveKinematics(ModuleParams *parent); - ~DifferentialDriveKinematics() = default; - - /** - * @brief Sets the wheel base of the robot. - * - * @param wheel_base The distance between the centers of the wheels. - */ - void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; - - /** - * @brief Sets the maximum speed of the robot. - * - * @param max_speed The maximum speed of the robot. - */ - 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; }; - - void setArmed(const bool armed) { _armed = armed; }; - - void allocate(); - - /** - * @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) const; - -private: - uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - - differential_drive_setpoint_s _differential_drive_control_output{}; - bool _armed = false; - - float _wheel_base{0.f}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; - - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp deleted file mode 100644 index b3c4935c1d..0000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 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 -#include "DifferentialDriveKinematics.hpp" -#include - -using namespace matrix; - -class DifferentialDriveKinematicsTest : public ::testing::Test -{ -public: - DifferentialDriveKinematics kinematics{nullptr}; -}; - -TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase) -{ - kinematics.setWheelBase(1.f); - 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_F(DifferentialDriveKinematicsTest, InvalidParameterCase) -{ - kinematics.setWheelBase(0.f); - 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_F(DifferentialDriveKinematicsTest, UnitCase) -{ - kinematics.setWheelBase(1.f); - 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_F(DifferentialDriveKinematicsTest, UnitSaturationCase) -{ - 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_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) -{ - 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_F(DifferentialDriveKinematicsTest, RandomCase) -{ - 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_F(DifferentialDriveKinematicsTest, RotateInPlaceCase) -{ - 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_F(DifferentialDriveKinematicsTest, StraightMovementCase) -{ - 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_F(DifferentialDriveKinematicsTest, MinInputValuesCase) -{ - 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_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) -{ - 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_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) -{ - 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_F(DifferentialDriveKinematicsTest, MaxAngularCase) -{ - 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/Kconfig b/src/modules/differential_drive/Kconfig deleted file mode 100644 index a95897e91f..0000000000 --- a/src/modules/differential_drive/Kconfig +++ /dev/null @@ -1,6 +0,0 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE - bool "differential_drive" - default n - depends on MODULES_CONTROL_ALLOCATOR - ---help--- - Enable support for control of differential drive rovers diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index fc187a954d..24b5004ee9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -58,8 +58,6 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_optional_topic("differential_drive_control_output", 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"); @@ -105,6 +103,8 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_optional_topic("rover_ackermann_guidance_status", 100); add_optional_topic("rover_ackermann_status", 100); + add_optional_topic("rover_differential_guidance_status", 100); + add_optional_topic("rover_differential_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt similarity index 84% rename from src/modules/differential_drive/CMakeLists.txt rename to src/modules/rover_differential/CMakeLists.txt index 2e0e3583b9..beaec32a87 100644 --- a/src/modules/differential_drive/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,22 +31,19 @@ # ############################################################################ -add_subdirectory(DifferentialDriveControl) -add_subdirectory(DifferentialDriveGuidance) -add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(RoverDifferentialGuidance) px4_add_module( - MODULE modules__differential_drive - MAIN differential_drive + MODULE modules__rover_differential + MAIN rover_differential SRCS - DifferentialDrive.cpp - DifferentialDrive.hpp + RoverDifferential.cpp + RoverDifferential.hpp DEPENDS - DifferentialDriveControl - DifferentialDriveGuidance - DifferentialDriveKinematics + RoverDifferentialGuidance px4_work_queue modules__control_allocator # for parameter CA_R_REV + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig new file mode 100644 index 0000000000..840e2cdbf9 --- /dev/null +++ b/src/modules/rover_differential/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_DIFFERENTIAL + bool "rover_differential" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp new file mode 100644 index 0000000000..ab89392a0e --- /dev/null +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "RoverDifferential.hpp" +using namespace matrix; +using namespace time_literals; + +RoverDifferential::RoverDifferential() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_differential_status_pub.advertise(); + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +bool RoverDifferential::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverDifferential::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + pid_set_parameters(&_pid_yaw_rate, + _param_rd_p_gain_yaw_rate.get(), // Proportional gain + _param_rd_i_gain_yaw_rate.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + +} + +void RoverDifferential::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + } + + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + + } + + _differential_setpoint.closed_loop_yaw_rate = false; + } break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + } + + _differential_setpoint.closed_loop_yaw_rate = true; + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, + _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _differential_setpoint.throttle = 0.f; + _differential_setpoint.yaw_rate = 0.f; + _differential_setpoint.closed_loop_yaw_rate = false; + break; + } + + float speed_diff_normalized = _differential_setpoint.yaw_rate; + + // Closed loop yaw rate control + if (_differential_setpoint.closed_loop_yaw_rate) { + if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), + _param_rd_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { + pid_reset_integral(&_pid_yaw_rate); + } + + // Publish rover differential status (logging) + rover_differential_status_s rover_differential_status{}; + rover_differential_status.timestamp = _timestamp; + rover_differential_status.actual_speed = _vehicle_forward_speed; + rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate; + rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; + rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + _rover_differential_status_pub.publish(rover_differential_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff) +{ + float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff); + + if (combined_velocity > 1.0f) { // Prioritize yaw rate + float excess_velocity = fabsf(combined_velocity - 1.0f); + forward_speed -= sign(forward_speed) * excess_velocity; + } + + // Calculate the left and right wheel speeds + return Vector2f(forward_speed - speed_diff, + forward_speed + speed_diff); +} + +int RoverDifferential::task_spawn(int argc, char *argv[]) +{ + RoverDifferential *instance = new RoverDifferential(); + + 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 RoverDifferential::custom_command(int argc, char *argv[]) +{ + return print_usage("unk_timestampn command"); +} + +int RoverDifferential::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Differential controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_differential_main(int argc, char *argv[]) +{ + return RoverDifferential::main(argc, argv); +} diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/rover_differential/RoverDifferential.hpp similarity index 53% rename from src/modules/differential_drive/DifferentialDrive.hpp rename to src/modules/rover_differential/RoverDifferential.hpp index 255bb0c526..3dafe99b05 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -33,31 +33,41 @@ #pragma once +// PX4 includes #include #include #include #include #include + +// uORB includes #include +#include #include -#include #include #include -#include #include +#include +#include +#include +#include +#include -#include "DifferentialDriveControl/DifferentialDriveControl.hpp" -#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" -#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp" +// Standard libraries +#include +#include + +// Local includes +#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" using namespace time_literals; -class DifferentialDrive : public ModuleBase, public ModuleParams, +class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - DifferentialDrive(); - ~DifferentialDrive() override = default; + RoverDifferential(); + ~RoverDifferential() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -70,36 +80,57 @@ public: bool init(); + /** + * @brief Computes motor commands for differential drive. + * + * @param forward_speed Linear velocity along the x-axis. + * @param speed_diff Speed difference between left and right wheels. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff); + protected: void updateParams() override; private: void Run() override; + + // uORB Subscriptions 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_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - bool _manual_driving = false; - bool _mission_driving = false; - bool _acro_driving = false; - hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + // uORB Publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - DifferentialDriveGuidance _differential_drive_guidance{this}; - DifferentialDriveControl _differential_drive_control{this}; - DifferentialDriveKinematics _differential_drive_kinematics{this}; + // Instances + RoverDifferentialGuidance _rover_differential_guidance{this}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; + // Variables + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + int _nav_state{0}; + matrix::Quatf _vehicle_attitude_quaternion{}; + hrt_abstime _timestamp{0}; + PID_t _pid_yaw_rate; // The PID controller for yaw rate + RoverDifferentialGuidance::differential_setpoint _differential_setpoint; + + // Constants + static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_speed_scale, - (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_speed, - (ParamFloat) _param_rdd_wheel_radius, - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_rd_man_yaw_scale, + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_p_gain_yaw_rate, + (ParamFloat) _param_rd_i_gain_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt similarity index 89% rename from src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt rename to src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index fa833c50ae..0fd7b68c39 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(DifferentialDriveGuidance - DifferentialDriveGuidance.cpp +px4_add_library(RoverDifferentialGuidance + RoverDifferentialGuidance.cpp ) -target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RoverDifferentialGuidance PUBLIC pid) +target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp new file mode 100644 index 0000000000..efad52e7b8 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "RoverDifferentialGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_differential_guidance_status_pub.advertise(); + pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + +} + +void RoverDifferentialGuidance::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_heading, + _param_rd_p_gain_heading.get(), // Proportional gain + _param_rd_i_gain_heading.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rd_p_gain_speed.get(), // Proportional gain + _param_rd_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit +} + +RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw, + const float actual_speed, const int nav_state) +{ + // Initializations + bool mission_finished{false}; + float desired_speed{0.f}; + float desired_yaw_rate{0.f}; + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + mission_finished = mission_result.finished; + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + math::max(actual_speed, 0.f)); + + const float heading_error = matrix::wrap_pi(desired_heading - yaw); + + const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), + _curr_wp(1)); + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch + mission_finished = true; + } + + // State machine + if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + desired_speed = _param_rd_miss_spd_def.get(); + + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_accel.get(), distance_to_next_wp, 0.0f); + desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + } + + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); + } break; + + case GuidanceState::SPOT_TURNING: + if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot + } + + break; + + case GuidanceState::STOPPED: + default: + desired_speed = 0.f; + desired_yaw_rate = 0.f; + break; + + } + + // Closed loop speed control + float throttle{0.f}; + + if (fabsf(desired_speed) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); + + if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term + throttle += math::interpolate(desired_speed, + 0.f, _param_rd_max_speed.get(), + 0.f, 1.f); + } + } + + // Publish differential controller status (logging) + _rover_differential_guidance_status.timestamp = _timestamp; + _rover_differential_guidance_status.desired_speed = desired_speed; + _rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); + _rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral; + _rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; + _rover_differential_guidance_status.state_machine = (uint8_t) _currentState; + _rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status); + + // Return setpoints + differential_setpoint differential_setpoint_temp; + differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); + differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate, + _max_yaw_rate); + differential_setpoint_temp.closed_loop_yaw_rate = true; + return differential_setpoint_temp; +} + +void RoverDifferentialGuidance::updateWaypoints() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + _curr_wp = Vector2d(0, 0); + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + + } else { + _prev_wp = _curr_pos; + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + _next_wp = _home_position; + } + + // NED waypoint coordinates + _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); + +} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp new file mode 100644 index 0000000000..29a131f8d8 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential rover guidance. + */ +class RoverDifferentialGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverDifferentialGuidance. + * @param parent The parent ModuleParams object. + */ + RoverDifferentialGuidance(ModuleParams *parent); + ~RoverDifferentialGuidance() = default; + + struct differential_setpoint { + float throttle{0.f}; + float yaw_rate{0.f}; + bool closed_loop_yaw_rate{false}; + }; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param actual_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, + int nav_state); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + + // uORB publications + uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; + rover_differential_guidance_status_s _rover_differential_guidance_status{}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + + // Waypoints + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + Vector2d _prev_wp{}; + Vector2f _prev_wp_ned{}; + Vector2d _curr_wp{}; + Vector2f _curr_wp_ned{}; + Vector2d _next_wp{}; + Vector2d _home_position{}; + + // Controllers + PID_t _pid_heading; // The PID controller for the heading + PID_t _pid_throttle; // The PID controller for velocity + + // Constants + static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_p_gain_heading, + (ParamFloat) _param_rd_i_gain_heading, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rd_max_jerk, + (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_miss_spd_def, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn + + ) +}; diff --git a/src/modules/differential_drive/module.yaml b/src/modules/rover_differential/module.yaml similarity index 55% rename from src/modules/differential_drive/module.yaml rename to src/modules/rover_differential/module.yaml index 50ab9bb837..b6819305bc 100644 --- a/src/modules/differential_drive/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -1,11 +1,12 @@ -module_name: Differential Drive +module_name: Rover Differential parameters: - - group: Rover Differential Drive + - group: Rover Differential definitions: - RDD_WHEEL_BASE: + + RD_WHEEL_TRACK: description: - short: Wheel base + short: Wheel track long: Distance from the center of the right wheel to the center of the left wheel type: float unit: m @@ -14,46 +15,21 @@ parameters: increment: 0.001 decimal: 3 default: 0.5 - RDD_WHEEL_RADIUS: + + RD_MAN_YAW_SCALE: description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel + short: Manual yaw rate scale + long: | + In manual mode the setpoint for the yaw rate received from the rc remote + is scaled by this value. 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 + min: 0.01 max: 1 increment: 0.01 decimal: 2 default: 1 - RDD_ANG_SCALE: - description: - short: Manual angular velocity scale - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RDD_WHEEL_SPEED: - description: - short: Maximum wheel speed - type: float - unit: rad/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.3 - RDD_P_HEADING: + + RD_HEADING_P: description: short: Proportional gain for heading controller type: float @@ -62,7 +38,18 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_P_SPEED: + + RD_HEADING_I: + description: + short: Integral gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.1 + + RD_SPEED_P: description: short: Proportional gain for speed controller type: float @@ -71,7 +58,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_SPEED: + + RD_SPEED_I: description: short: Integral gain for ground speed controller type: float @@ -80,7 +68,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_P_ANG_VEL: + + RD_YAW_RATE_P: description: short: Proportional gain for angular velocity controller type: float @@ -89,7 +78,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_ANG_VEL: + + RD_YAW_RATE_I: description: short: Integral gain for angular velocity controller type: float @@ -98,7 +88,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_MAX_JERK: + + RD_MAX_JERK: description: short: Maximum jerk long: Limit for forwards acc/deceleration change. @@ -109,7 +100,8 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 - RDD_MAX_ACCEL: + + RD_MAX_ACCEL: description: short: Maximum acceleration long: Maximum acceleration is used to limit the acceleration of the rover @@ -120,3 +112,61 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 + + RD_MAX_SPEED: + description: + short: Maximum speed the rover can drive + long: This parameter is used to map desired speeds to normalized motor commands. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 7 + + RD_MAX_YAW_RATE: + description: + short: Maximum allowed yaw rate for the rover. + long: | + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + type: float + unit: deg/s + min: 0.01 + max: 1000 + increment: 0.01 + decimal: 2 + default: 90 + + RD_MISS_SPD_DEF: + description: + short: Default rover speed during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RD_TRANS_TRN_DRV: + description: + short: Heading error threshhold to switch from spot turning to driving + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.0872665 + + RD_TRANS_DRV_TRN: + description: + short: Heading error threshhold to switch from driving to spot turning + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.174533 diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 67b1e9456c..e9e67cff25 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -130,9 +130,6 @@ 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