From fb05dedb7b72982cbfc8e2d1d73e513076f184d3 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 5 Sep 2025 13:28:30 +0200 Subject: [PATCH] sv: extend rover control lib to surface vehicles --- src/lib/CMakeLists.txt | 2 +- .../CMakeLists.txt | 12 +++++----- .../SurfaceVehicleControl.cpp} | 6 ++--- .../SurfaceVehicleControl.hpp} | 15 +++++++------ .../SurfaceVehicleControlTest.cpp} | 22 +++++++++---------- .../rovercontrol_params.c | 0 .../AckermannActControl.cpp | 2 +- .../AckermannActControl.hpp | 2 +- .../AckermannAttControl.cpp | 2 +- .../AckermannAttControl.hpp | 2 +- .../AckermannAutoMode/AckermannAutoMode.cpp | 5 +++-- .../AckermannAutoMode/AckermannAutoMode.hpp | 2 +- .../AckermannManualMode.hpp | 2 +- .../AckermannRateControl.hpp | 2 +- .../AckermannSpeedControl.cpp | 3 ++- .../AckermannSpeedControl.hpp | 2 +- src/modules/rover_ackermann/CMakeLists.txt | 2 +- src/modules/rover_differential/CMakeLists.txt | 2 +- .../DifferentialActControl.cpp | 2 +- .../DifferentialActControl.hpp | 2 +- .../DifferentialAttControl.cpp | 2 +- .../DifferentialAttControl.hpp | 2 +- .../DifferentialAutoMode.cpp | 5 +++-- .../DifferentialAutoMode.hpp | 2 +- .../DifferentialRateControl.cpp | 2 +- .../DifferentialRateControl.hpp | 2 +- .../DifferentialSpeedControl.cpp | 3 ++- .../DifferentialSpeedControl.hpp | 2 +- src/modules/rover_mecanum/CMakeLists.txt | 2 +- .../MecanumActControl/MecanumActControl.cpp | 4 ++-- .../MecanumActControl/MecanumActControl.hpp | 2 +- .../MecanumAttControl/MecanumAttControl.cpp | 2 +- .../MecanumAttControl/MecanumAttControl.hpp | 2 +- .../MecanumAutoMode/MecanumAutoMode.cpp | 5 +++-- .../MecanumAutoMode/MecanumAutoMode.hpp | 2 +- .../MecanumRateControl/MecanumRateControl.cpp | 2 +- .../MecanumRateControl/MecanumRateControl.hpp | 2 +- .../MecanumSpeedControl.cpp | 6 +++-- .../MecanumSpeedControl.hpp | 2 +- 39 files changed, 74 insertions(+), 66 deletions(-) rename src/lib/{rover_control => surface_vehicle_control}/CMakeLists.txt (85%) rename src/lib/{rover_control/RoverControl.cpp => surface_vehicle_control/SurfaceVehicleControl.cpp} (99%) rename src/lib/{rover_control/RoverControl.hpp => surface_vehicle_control/SurfaceVehicleControl.hpp} (93%) rename src/lib/{rover_control/RoverControlTest.cpp => surface_vehicle_control/SurfaceVehicleControlTest.cpp} (75%) rename src/lib/{rover_control => surface_vehicle_control}/rovercontrol_params.c (100%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8afa75fd58..633eaafe80 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -67,11 +67,11 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) -add_subdirectory(rover_control EXCLUDE_FROM_ALL) add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(stick_yaw EXCLUDE_FROM_ALL) +add_subdirectory(surface_vehicle_control EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) add_subdirectory(system_identification EXCLUDE_FROM_ALL) add_subdirectory(tecs EXCLUDE_FROM_ALL) diff --git a/src/lib/rover_control/CMakeLists.txt b/src/lib/surface_vehicle_control/CMakeLists.txt similarity index 85% rename from src/lib/rover_control/CMakeLists.txt rename to src/lib/surface_vehicle_control/CMakeLists.txt index f3a9277d29..a2e20a9ecc 100644 --- a/src/lib/rover_control/CMakeLists.txt +++ b/src/lib/surface_vehicle_control/CMakeLists.txt @@ -31,11 +31,11 @@ # ############################################################################ -px4_add_library(rover_control - RoverControl.cpp - RoverControl.hpp +px4_add_library(surface_vehicle_control + SurfaceVehicleControl.cpp + SurfaceVehicleControl.hpp ) -target_link_libraries(rover_control PUBLIC PID) -target_link_libraries(rover_control PUBLIC geo) -px4_add_unit_gtest(SRC RoverControlTest.cpp LINKLIBS rover_control) +target_link_libraries(surface_vehicle_control PUBLIC PID) +target_link_libraries(surface_vehicle_control PUBLIC geo) +px4_add_unit_gtest(SRC SurfaceVehicleControlTest.cpp LINKLIBS surface_vehicle_control) diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/surface_vehicle_control/SurfaceVehicleControl.cpp similarity index 99% rename from src/lib/rover_control/RoverControl.cpp rename to src/lib/surface_vehicle_control/SurfaceVehicleControl.cpp index 68d06c7d2b..fb770e8371 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/surface_vehicle_control/SurfaceVehicleControl.cpp @@ -31,9 +31,9 @@ * ****************************************************************************/ -#include "RoverControl.hpp" +#include "SurfaceVehicleControl.hpp" using namespace matrix; -namespace RoverControl +namespace SurfaceVehicleControl { float throttleControl(SlewRate &motor_setpoint, const float throttle_setpoint, const float current_motor_setpoint, const float max_accel, const float max_decel, const float max_thr_spd, @@ -265,4 +265,4 @@ float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, return acosf(cosin); } -} // RoverControl +} // SurfaceVehicleControl diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/surface_vehicle_control/SurfaceVehicleControl.hpp similarity index 93% rename from src/lib/rover_control/RoverControl.hpp rename to src/lib/surface_vehicle_control/SurfaceVehicleControl.hpp index 97f38f21f4..a34d89f8b0 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/surface_vehicle_control/SurfaceVehicleControl.hpp @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * @file RoverControl.hpp + * @file SurfaceVehicleControl.hpp * - * Functions that are shared among the different rover modules. - * Also includes the parameters that are shared among the rover modules. + * Functions that are shared among the different surface vehicles. + * Also includes the parameters that are shared among them. */ #pragma once @@ -47,7 +47,7 @@ #include using namespace matrix; -namespace RoverControl +namespace SurfaceVehicleControl { /** * Applies acceleration/deceleration slew rate to a throttle setpoint. @@ -56,7 +56,7 @@ namespace RoverControl * @param current_motor_setpoint Currently applied motor input [-1, 1] * @param max_accel Maximum allowed acceleration [m/s^2] * @param max_decel Maximum allowed deceleration [m/s^2] - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s] + * @param max_thr_spd Speed the vehicle drives at maximum throttle [m/s] * @param dt Time since last update [s] * @return Motor Setpoint [-1, 1] */ @@ -76,6 +76,7 @@ float throttleControl(SlewRate &motor_setpoint, float throttle_setpoint, */ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, float yaw_slew_rate, float vehicle_yaw, float yaw_setpoint, float dt); + /** * Applies acceleration/deceleration slew rate to a speed setpoint and calculates the necessary throttle setpoint * using a feed forward term and PID controller. @@ -95,7 +96,7 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float /** * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint * using a feedforward term and/or a PID controller. - * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * Note: This function is only for vehicles that control the rate through a speed difference between the left/right wheels. * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). * @param pid_yaw_rate Yaw rate PID (Updated by this function). * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. @@ -119,7 +120,7 @@ float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate * @param prev_wp_ned Previous waypoint in NED frame (Updated by this function) * @param next_wp_ned Next waypoint in NED frame (Updated by this function) * @param position_setpoint_triplet Position Setpoint Triplet - * @param curr_pos Current position of the rover in global frame + * @param curr_pos Current position of the vehicle in global frame * @param global_ned_proj_ref Global to ned projection */ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, diff --git a/src/lib/rover_control/RoverControlTest.cpp b/src/lib/surface_vehicle_control/SurfaceVehicleControlTest.cpp similarity index 75% rename from src/lib/rover_control/RoverControlTest.cpp rename to src/lib/surface_vehicle_control/SurfaceVehicleControlTest.cpp index 508cdd4486..3b3b1723f1 100644 --- a/src/lib/rover_control/RoverControlTest.cpp +++ b/src/lib/surface_vehicle_control/SurfaceVehicleControlTest.cpp @@ -33,24 +33,24 @@ /****************************************************************** * Test code for the Pure Pursuit algorithm - * Run this test only using "make tests TESTFILTER=RoverControl" + * Run this test only using "make tests TESTFILTER=SurfaceVehicleControl" ******************************************************************/ #include -#include "RoverControl.hpp" +#include "SurfaceVehicleControl.hpp" TEST(calcWaypointTransitionAngle, invalidInputs) { Vector2f prev_wp_ned(NAN, NAN); Vector2f curr_wp_ned(10.f, 10.f); Vector2f next_wp_ned(10.f, 10.f); - float prevInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + float prevInvalid = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); prev_wp_ned = Vector2f(10.f, 10.f); curr_wp_ned = Vector2f(NAN, NAN); - float currInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + float currInvalid = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); curr_wp_ned = Vector2f(10.f, 10.f); next_wp_ned = Vector2f(NAN, NAN); - float nextInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + float nextInvalid = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FALSE(PX4_ISFINITE(prevInvalid)); EXPECT_FALSE(PX4_ISFINITE(currInvalid)); EXPECT_FALSE(PX4_ISFINITE(nextInvalid)); @@ -63,7 +63,7 @@ TEST(calcWaypointTransitionAngle, validInputs) Vector2f prev_wp_ned(0.f, 0.f); Vector2f curr_wp_ned(10.f, 0.f); Vector2f next_wp_ned(20.f, 0.f); - const float angle1 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle1 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FLOAT_EQ(angle1, M_PI_F); /** @@ -74,7 +74,7 @@ TEST(calcWaypointTransitionAngle, validInputs) prev_wp_ned = Vector2f(0.f, 0.f); curr_wp_ned = Vector2f(10.f, 0.f); next_wp_ned = Vector2f(20.f, 10.f); - const float angle2 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle2 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FLOAT_EQ(angle2, M_PI_F - M_PI_4_F); /** @@ -85,7 +85,7 @@ TEST(calcWaypointTransitionAngle, validInputs) prev_wp_ned = Vector2f(0.f, 0.f); curr_wp_ned = Vector2f(10.f, 0.f); next_wp_ned = Vector2f(10.f, 10.f); - const float angle3 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle3 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FLOAT_EQ(angle3, M_PI_2_F); /** @@ -96,20 +96,20 @@ TEST(calcWaypointTransitionAngle, validInputs) prev_wp_ned = Vector2f(0.f, 0.f); curr_wp_ned = Vector2f(10.f, 0.f); next_wp_ned = Vector2f(0.f, 10.f); - const float angle4 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle4 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FLOAT_EQ(angle4, M_PI_4_F); // P/C -- N prev_wp_ned = Vector2f(0.f, 0.f); curr_wp_ned = Vector2f(0.f, 0.f); next_wp_ned = Vector2f(10.f, 0.f); - const float angle5 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle5 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FALSE(PX4_ISFINITE(angle5)); // P -- C/N prev_wp_ned = Vector2f(0.f, 0.f); curr_wp_ned = Vector2f(10.f, 0.f); next_wp_ned = Vector2f(10.f, 0.f); - const float angle6 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + const float angle6 = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); EXPECT_FALSE(PX4_ISFINITE(angle6)); } diff --git a/src/lib/rover_control/rovercontrol_params.c b/src/lib/surface_vehicle_control/rovercontrol_params.c similarity index 100% rename from src/lib/rover_control/rovercontrol_params.c rename to src/lib/surface_vehicle_control/rovercontrol_params.c diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp index 159feeb7f3..8199ea73fe 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -71,7 +71,7 @@ void AckermannActControl::updateActControl() _actuator_motors_sub.copy(&actuator_motors_sub); actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, + actuator_motors.control[0] = SurfaceVehicleControl::throttleControl(_motor_setpoint, _throttle_setpoint, actuator_motors_sub.control[0], _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); actuator_motors.timestamp = _timestamp; diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp index a63266c02e..b3629e9e8c 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include #include diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index 2e85e5f6c7..514721fd9d 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -73,7 +73,7 @@ void AckermannAttControl::updateAttControl() _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate); - float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, + float yaw_rate_setpoint = SurfaceVehicleControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, _vehicle_yaw, _yaw_setpoint, dt); surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp index 78f674299f..696d41f140 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -37,7 +37,7 @@ #include // Library includes -#include +#include #include #include #include diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp index b9a98d49c4..295a91c736 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp @@ -92,10 +92,11 @@ void AckermannAutoMode::updateWaypointsAndAcceptanceRadius() _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); _curr_wp_type = position_setpoint_triplet.current.type; - RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + SurfaceVehicleControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, _curr_pos_ned, _global_ned_proj_ref); - _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + _waypoint_transition_angle = SurfaceVehicleControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, + _next_wp_ned); // Update acceptance radius if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp index 0b0397f5ba..3ab7c1ee4d 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include // uORB includes diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp index 9c50a6a70c..15e605ea7a 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include // uORB includes diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index 925b0e68d9..e87ba1752f 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp index a669d27de3..a523206401 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp @@ -71,7 +71,8 @@ void AckermannSpeedControl::updateSpeedControl() _param_ro_speed_limit.get()); surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = _timestamp; - surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, + surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_setpoint, + _pid_speed, speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); surface_vehicle_throttle_setpoint.throttle_body_y = NAN; diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp index e65ef2da59..7f39117ff2 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp @@ -38,7 +38,7 @@ #include // Library includes -#include +#include #include #include #include diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 79dce06543..52db207430 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -54,7 +54,7 @@ px4_add_module( AckermannManualMode AckermannOffboardMode px4_work_queue - rover_control + surface_vehicle_control pure_pursuit MODULE_CONFIG module.yaml diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 10d7d551a1..60dca47ef6 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -54,7 +54,7 @@ px4_add_module( DifferentialManualMode DifferentialOffboardMode px4_work_queue - rover_control + surface_vehicle_control pure_pursuit MODULE_CONFIG module.yaml diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp index c44951fc68..2f24877e3a 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -72,7 +72,7 @@ void DifferentialActControl::updateActControl() actuator_motors_s actuator_motors_sub{}; _actuator_motors_sub.copy(&actuator_motors_sub); const float current_throttle = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; - const float adjusted_throttle_setpoint = RoverControl::throttleControl(_adjusted_throttle_setpoint, + const float adjusted_throttle_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_setpoint, _throttle_setpoint, current_throttle, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); actuator_motors_s actuator_motors{}; diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp index 30c84a5cdf..dc532de523 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include #include diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp index 332c7bdd07..e737513e6c 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -79,7 +79,7 @@ void DifferentialAttControl::updateAttControl() } if (PX4_ISFINITE(_yaw_setpoint)) { - const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + const float yaw_rate_setpoint = SurfaceVehicleControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, _vehicle_yaw, _yaw_setpoint, dt); surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = _timestamp; diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp index c48176c923..40dfd3a613 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp index a2c4e8d81b..dd65a4d230 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp @@ -69,10 +69,11 @@ void DifferentialAutoMode::autoControl() Vector2f prev_wp_ned{NAN, NAN}; Vector2f next_wp_ned{NAN, NAN}; - RoverControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet, + SurfaceVehicleControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet, curr_pos_ned, global_ned_proj_ref); - float waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + float waypoint_transition_angle = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, + next_wp_ned); // Waypoint cruising speed float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp index dcd3f5c97d..a853c5225e 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include // uORB includes diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index ea2a59b427..584592331d 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -77,7 +77,7 @@ void DifferentialRateControl::updateRateControl() if (PX4_ISFINITE(_yaw_rate_setpoint)) { const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; - const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + const float speed_diff_normalized = SurfaceVehicleControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt); diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index 535282fffd..7ca00899a8 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp index 5707691b17..652c16afbc 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp @@ -70,7 +70,8 @@ void DifferentialSpeedControl::updateSpeedControl() const float speed_setpoint = calcSpeedSetpoint(); surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = _timestamp; - surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, + surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_setpoint, + _pid_speed, speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp index 3454794709..f7d245301f 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 295910c7d4..1717ee0b7f 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -54,7 +54,7 @@ px4_add_module( MecanumManualMode MecanumOffboardMode px4_work_queue - rover_control + surface_vehicle_control pure_pursuit MODULE_CONFIG module.yaml diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp index 379820d420..f18c8621a0 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp @@ -75,10 +75,10 @@ void MecanumActControl::updateActControl() _actuator_motors_sub.copy(&actuator_motors_sub); const float current_throttle_x = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; const float current_throttle_y = (actuator_motors_sub.control[2] - actuator_motors_sub.control[0]) / 2.f; - const float adjusted_throttle_x_setpoint = RoverControl::throttleControl(_adjusted_throttle_x_setpoint, + const float adjusted_throttle_x_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_x_setpoint, _throttle_x_setpoint, current_throttle_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - const float adjusted_throttle_y_setpoint = RoverControl::throttleControl(_adjusted_throttle_y_setpoint, + const float adjusted_throttle_y_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_y_setpoint, _throttle_y_setpoint, current_throttle_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); actuator_motors_s actuator_motors{}; diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp index 8abca5907e..584ea6fb2f 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include #include diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 643fc58601..5c7659fecf 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -79,7 +79,7 @@ void MecanumAttControl::updateAttControl() } if (PX4_ISFINITE(_yaw_setpoint)) { - const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + const float yaw_rate_setpoint = SurfaceVehicleControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, _vehicle_yaw, _yaw_setpoint, dt); surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = _timestamp; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 8833cf73d0..6a8f3f5518 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp index 2db1f91e9f..091e8cd21b 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp @@ -69,10 +69,11 @@ void MecanumAutoMode::autoControl() Vector2f prev_wp_ned{NAN, NAN}; Vector2f next_wp_ned{NAN, NAN}; - RoverControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet, + SurfaceVehicleControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet, curr_pos_ned, global_ned_proj_ref); - float waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + float waypoint_transition_angle = SurfaceVehicleControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, + next_wp_ned); // Waypoint cruising speed float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp index f3dd02291d..8fe904d4bc 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp @@ -37,7 +37,7 @@ #include // Libraries -#include +#include #include // uORB includes diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index b2e80693f2..6aa80e06c0 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -77,7 +77,7 @@ void MecanumRateControl::updateRateControl() if (PX4_ISFINITE(_yaw_rate_setpoint)) { const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; - const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + const float speed_diff_normalized = SurfaceVehicleControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt); diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index ed0d0bcdf6..199a6cf8b7 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp index 0afe207016..9ce3d1d0aa 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp @@ -75,10 +75,12 @@ void MecanumSpeedControl::updateSpeedControl() surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = _timestamp; - surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_x_setpoint, _pid_speed_x, + surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_x_setpoint, + _pid_speed_x, speed_setpoint(0), _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - surface_vehicle_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y, + surface_vehicle_throttle_setpoint.throttle_body_y = SurfaceVehicleControl::speedControl(_adjusted_speed_y_setpoint, + _pid_speed_y, speed_setpoint(1), _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp index 479cd0446a..25db02a7d8 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp @@ -38,7 +38,7 @@ #include // Libraries -#include +#include #include #include #include