From 59099d7c690977180d13224c00876d32ce12e137 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Wed, 13 Mar 2024 13:58:37 +0100 Subject: [PATCH] differential: restructure and update module --- .../init.d-posix/airframes/4009_gz_r1_rover | 24 +- .../init.d-posix/airframes/4011_gz_lawnmower | 12 +- .../init.d/rc.rover_differential_defaults | 11 +- boards/px4/fmu-v5/rover.px4board | 1 + boards/px4/fmu-v5x/rover.px4board | 1 + boards/px4/fmu-v6c/rover.px4board | 1 + boards/px4/fmu-v6u/rover.px4board | 1 + boards/px4/fmu-v6x/rover.px4board | 1 + boards/px4/fmu-v6xrt/rover.px4board | 1 + msg/CMakeLists.txt | 2 + msg/RoverDifferentialGuidanceStatus.msg | 10 + msg/RoverDifferentialSetpoint.msg | 8 - msg/RoverDifferentialStatus.msg | 8 + src/modules/logger/logged_topics.cpp | 2 + src/modules/rover_differential/CMakeLists.txt | 5 +- .../rover_differential/RoverDifferential.cpp | 171 +++++++++---- .../rover_differential/RoverDifferential.hpp | 73 ++++-- .../RoverDifferentialControl/CMakeLists.txt | 39 --- .../RoverDifferentialControl.cpp | 109 --------- .../RoverDifferentialControl.hpp | 91 ------- .../RoverDifferentialGuidance/CMakeLists.txt | 1 + .../RoverDifferentialGuidance.cpp | 227 +++++++++++++----- .../RoverDifferentialGuidance.hpp | 135 ++++++----- .../CMakeLists.txt | 40 --- .../RoverDifferentialKinematics.cpp | 97 -------- .../RoverDifferentialKinematics.hpp | 103 -------- .../RoverDifferentialKinematicsTest.cpp | 168 ------------- src/modules/rover_differential/module.yaml | 126 +++++++--- src/modules/uxrce_dds_client/dds_topics.yaml | 3 - 29 files changed, 568 insertions(+), 903 deletions(-) create mode 100644 msg/RoverDifferentialGuidanceStatus.msg delete mode 100644 msg/RoverDifferentialSetpoint.msg create mode 100644 msg/RoverDifferentialStatus.msg delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialKinematics/CMakeLists.txt delete mode 100644 src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematicsTest.cpp 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 80664cf661c0..7697e23db769 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 1dcb917a09ad..01fc515c4370 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/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index e0cace3a3238..4cddd44f89e3 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 fcf98e28bfe1..07bfbe094121 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -19,3 +19,4 @@ 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-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 9f5d4e904a37..4a4458c2e6bc 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -16,3 +16,4 @@ 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 de938cc49856..7ded4008d8af 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -15,3 +15,4 @@ 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-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index de938cc49856..7ded4008d8af 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -15,3 +15,4 @@ 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-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index de938cc49856..7ded4008d8af 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -15,3 +15,4 @@ 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-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 5412ab334573..95a01552267c 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -16,3 +16,4 @@ 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/msg/CMakeLists.txt b/msg/CMakeLists.txt index ffb2b283fe59..f0f6d520c701 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -180,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/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg new file mode 100644 index 000000000000..836546e7ebb7 --- /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/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index 0aed73afcef8..000000000000 --- a/msg/RoverDifferentialSetpoint.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 rover_differential_setpoint rover_differential_control_output diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg new file mode 100644 index 000000000000..31907ffa6477 --- /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/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 45f7b1a99b5e..24b5004ee9af 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -103,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/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 5f1243120652..beaec32a8776 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,9 +31,7 @@ # ############################################################################ -add_subdirectory(RoverDifferentialControl) add_subdirectory(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialKinematics) px4_add_module( MODULE modules__rover_differential @@ -42,11 +40,10 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialControl RoverDifferentialGuidance - RoverDifferentialKinematics px4_work_queue modules__control_allocator # for parameter CA_R_REV + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 6b5b4b88eb4a..ab89392a0ec8 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -32,12 +32,16 @@ ****************************************************************************/ #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() @@ -50,15 +54,15 @@ void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _rover_differential_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _rover_differential_guidance.setMaxSpeed(_max_speed); - _rover_differential_kinematics.setMaxSpeed(_max_speed); + 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 - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); - _rover_differential_guidance.setMaxAngularVelocity(_max_angular_velocity); - _rover_differential_kinematics.setMaxAngularVelocity(_max_angular_velocity); } void RoverDifferential::Run() @@ -69,74 +73,135 @@ void RoverDifferential::Run() return; } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; - _time_stamp_last = now; + 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_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode{}; + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } - 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_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_status_sub.updated()) { - vehicle_status_s vehicle_status{}; + 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_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); - _rover_differential_kinematics.setArmed(spooled_up); - _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); - } + 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); } - if (_manual_driving) { - // Manual mode - // directly produce setpoints from the manual control setpoint (joystick) - if (_manual_control_setpoint_sub.updated()) { + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { manual_control_setpoint_s manual_control_setpoint{}; - if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - rover_differential_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 (_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(); + + } - // 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; + _differential_setpoint.closed_loop_yaw_rate = false; + } break; - } else { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = false; - } + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; - setpoint.timestamp = now; - _rover_differential_setpoint_pub.publish(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 if (_mission_driving) { - // Mission mode - // directly receive setpoints from the guidance library - _rover_differential_guidance.computeGuidance( - _rover_differential_control.getVehicleYaw(), - _rover_differential_control.getVehicleBodyYawRate(), - dt - ); + } 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; } - _rover_differential_control.control(dt); - _rover_differential_kinematics.allocate(); + // 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[]) @@ -164,7 +229,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unknown command"); + return print_usage("unk_timestampn command"); } int RoverDifferential::print_usage(const char *reason) diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 96eee58a92e7..3dafe99b0566 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -33,22 +33,32 @@ #pragma once +// PX4 includes #include #include #include #include #include + +// uORB includes #include +#include #include -#include #include #include -#include #include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include -#include "RoverDifferentialControl/RoverDifferentialControl.hpp" +// Local includes #include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialKinematics/RoverDifferentialKinematics.hpp" using namespace time_literals; @@ -70,36 +80,57 @@ class RoverDifferential : public ModuleBase, public ModulePar 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 _rover_differential_setpoint_pub{ORB_ID(rover_differential_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)}; + // Instances RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - RoverDifferentialKinematics _rover_differential_kinematics{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/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt deleted file mode 100644 index 58a5239f8cc7..000000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/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(RoverDifferentialControl - RoverDifferentialControl.cpp -) - -target_link_libraries(RoverDifferentialControl PUBLIC pid) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index e0f0482de1e5..000000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.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 "RoverDifferentialControl.hpp" - -using namespace matrix; - -RoverDifferentialControl::RoverDifferentialControl(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 RoverDifferentialControl::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 RoverDifferentialControl::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); - } - } - - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // PID to reach setpoint using control_output - rover_differential_setpoint_s rover_differential_control_output = _rover_differential_setpoint; - - if (_rover_differential_setpoint.closed_loop_speed_control) { - rover_differential_control_output.speed += - pid_calculate(&_pid_speed, _rover_differential_setpoint.speed, _vehicle_forward_speed, 0, dt); - } - - if (_rover_differential_setpoint.closed_loop_yaw_rate_control) { - rover_differential_control_output.yaw_rate += - pid_calculate(&_pid_angular_velocity, _rover_differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); - } - - rover_differential_control_output.timestamp = hrt_absolute_time(); - _rover_differential_control_output_pub.publish(rover_differential_control_output); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index 93062b8bc22b..000000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.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 RoverDifferentialControl.hpp - * - * Controller for heading rate and forward speed. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class RoverDifferentialControl : public ModuleParams -{ -public: - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = 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 _rover_differential_setpoint_sub{ORB_ID(rover_differential_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 _rover_differential_control_output_pub{ORB_ID(rover_differential_control_output)}; - - rover_differential_setpoint_s _rover_differential_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/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index 81f350d48403..0fd7b68c394e 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -35,4 +35,5 @@ px4_add_library(RoverDifferentialGuidance RoverDifferentialGuidance.cpp ) +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 index 9dd892a3345a..efad52e7b874 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -36,103 +36,212 @@ #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); - pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); } -void RoverDifferentialGuidance::computeGuidance(float yaw, float angular_velocity, float dt) +void RoverDifferentialGuidance::updateParams() { - if (_position_setpoint_triplet_sub.updated()) { - _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); - } + 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_sub.copy(&_vehicle_global_position); + 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); } - 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); + 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); + } - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), - current_waypoint(0), - current_waypoint(1)); + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } - 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 (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + mission_finished = mission_result.finished; + } - if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::TURNING; + 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); } - // 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; + 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; } - float desired_speed = 0.f; + // State machine + if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } - switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::SPOT_TURNING; - if (fabsf(heading_error) < 0.05f) { + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + pid_reset_integral(&_pid_heading); _currentState = GuidanceState::DRIVING; } - break; + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + // Guidance logic + switch (_currentState) { 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; + 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 } - case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle + break; + + case GuidanceState::STOPPED: + default: desired_speed = 0.f; - heading_error = 0.f; - angular_velocity = 0.f; - _desired_angular_velocity = 0.f; + desired_yaw_rate = 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; + // Closed loop speed control + float throttle{0.f}; + + if (fabsf(desired_speed) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); - rover_differential_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(); + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); - _rover_differential_setpoint_pub.publish(output); + 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); + } + } - _current_waypoint = current_waypoint; + // 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::updateParams() +void RoverDifferentialGuidance::updateWaypoints() { - ModuleParams::updateParams(); + 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; + } - 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 + // 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)); - _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/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index b058a8b251e7..29a131f8d8e2 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -33,37 +33,42 @@ #pragma once +// PX4 includes #include +#include -#include -#include -#include -#include -#include - -#include -#include - +// uORB includes #include #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 { - TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. + 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 drive guidance. + * @brief Class for differential rover guidance. */ class RoverDifferentialGuidance : public ModuleParams { @@ -75,32 +80,26 @@ class RoverDifferentialGuidance : public ModuleParams 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 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 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. */ - void computeGuidance(float yaw, float angular_velocity, float dt); + RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, + int nav_state); /** - * @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. + * @brief Update global/ned waypoint coordinates */ - 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; } + void updateWaypoints(); protected: /** @@ -109,32 +108,56 @@ class RoverDifferentialGuidance : public ModuleParams 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::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_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. - + 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_rdd_p_gain_heading, + (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_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel + (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/rover_differential/RoverDifferentialKinematics/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialKinematics/CMakeLists.txt deleted file mode 100644 index 78e6c3de363a..000000000000 --- a/src/modules/rover_differential/RoverDifferentialKinematics/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(RoverDifferentialKinematics - RoverDifferentialKinematics.cpp -) - -target_include_directories(RoverDifferentialKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_functional_gtest(SRC RoverDifferentialKinematicsTest.cpp LINKLIBS RoverDifferentialKinematics) diff --git a/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.cpp b/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.cpp deleted file mode 100644 index f4929bc34c09..000000000000 --- a/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.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 "RoverDifferentialKinematics.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialKinematics::RoverDifferentialKinematics(ModuleParams *parent) : ModuleParams(parent) -{} - -void RoverDifferentialKinematics::allocate() -{ - hrt_abstime now = hrt_absolute_time(); - - if (_rover_differential_control_output_sub.updated()) { - _rover_differential_control_output_sub.copy(&_rover_differential_control_output); - } - - const bool setpoint_timeout = (_rover_differential_control_output.timestamp + 100_ms) < now; - - Vector2f wheel_speeds = - computeInverseKinematics(_rover_differential_control_output.speed, _rover_differential_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 RoverDifferentialKinematics::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/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.hpp b/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.hpp deleted file mode 100644 index f19a32edf813..000000000000 --- a/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematics.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 RoverDifferentialKinematics : public ModuleParams -{ -public: - RoverDifferentialKinematics(ModuleParams *parent); - ~RoverDifferentialKinematics() = 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 _rover_differential_control_output_sub{ORB_ID(rover_differential_control_output)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - - rover_differential_setpoint_s _rover_differential_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/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematicsTest.cpp b/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematicsTest.cpp deleted file mode 100644 index 0645b383eabb..000000000000 --- a/src/modules/rover_differential/RoverDifferentialKinematics/RoverDifferentialKinematicsTest.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 "RoverDifferentialKinematics.hpp" -#include - -using namespace matrix; - -class RoverDifferentialKinematicsTest : public ::testing::Test -{ -public: - RoverDifferentialKinematics kinematics{nullptr}; -}; - -TEST_F(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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(RoverDifferentialKinematicsTest, 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/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 3944a3ccfd13..b6819305bc5f 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,9 +3,10 @@ module_name: Rover Differential parameters: - 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,55 +15,41 @@ parameters: increment: 0.001 decimal: 3 default: 0.5 - RDD_WHEEL_RADIUS: - description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.1 - RDD_SPEED_SCALE: - description: - short: Manual speed scale - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RDD_ANG_SCALE: + + RD_MAN_YAW_SCALE: description: - short: Manual angular velocity scale + 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 - min: 0 + min: 0.01 max: 1 increment: 0.01 decimal: 2 default: 1 - RDD_WHEEL_SPEED: + + RD_HEADING_P: description: - short: Maximum wheel speed + short: Proportional gain for heading controller type: float - unit: rad/s min: 0 max: 100 increment: 0.01 decimal: 2 - default: 0.3 - RDD_P_HEADING: + default: 1 + + RD_HEADING_I: description: - short: Proportional gain for heading controller + short: Integral gain for heading controller type: float min: 0 max: 100 increment: 0.01 decimal: 2 - default: 1 - RDD_P_SPEED: + 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 2bd5f9ec1bd5..e9e67cff25ed 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/rover_differential_setpoint - type: px4_msgs::msg::RoverDifferentialSetpoint - - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry