fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

sv: extend rover control lib to surface vehicles

Browse Source
This commit is contained in:
chfriedrich98 2025-09-05 13:28:30 +02:00
parent ae65b33ba0
commit fb05dedb7b
39 changed files with 74 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/CMakeLists.txt
View File

@ -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)

12
src/lib/rover_control/CMakeLists.txt → src/lib/surface_vehicle_control/CMakeLists.txt
View File

@ -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)

6
src/lib/rover_control/RoverControl.cpp → src/lib/surface_vehicle_control/SurfaceVehicleControl.cpp
View File

@ -31,9 +31,9 @@
*
****************************************************************************/
#include "RoverControl.hpp"
#include "SurfaceVehicleControl.hpp"
using namespace matrix;
namespace RoverControl
namespace SurfaceVehicleControl
{
float throttleControl(SlewRate<float> &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

15
src/lib/rover_control/RoverControl.hpp → src/lib/surface_vehicle_control/SurfaceVehicleControl.hpp
View File

@ -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 <lib/geo/geo.h>
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<float> &motor_setpoint, float throttle_setpoint,
*/
float attitudeControl(SlewRateYaw<float> &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<float> &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<float> &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,

22
src/lib/rover_control/RoverControlTest.cpp → src/lib/surface_vehicle_control/SurfaceVehicleControlTest.cpp
View File

@ -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 <gtest/gtest.h>
#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));
}

0
src/lib/rover_control/rovercontrol_params.c → src/lib/surface_vehicle_control/rovercontrol_params.c
View File

2
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp
View File

@ -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;

2
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

2
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
View File

@ -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{};

2
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Library includes
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <math.h>

5
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp
View File

@ -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()) {

2
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes

2
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes

2
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

3
src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp
View File

@ -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;

2
src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Library includes
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/slew_rate/SlewRate.hpp>

2
src/modules/rover_ackermann/CMakeLists.txt
View File

@ -54,7 +54,7 @@ px4_add_module(
AckermannManualMode
AckermannOffboardMode
px4_work_queue
rover_control
surface_vehicle_control
pure_pursuit
MODULE_CONFIG
module.yaml

2
src/modules/rover_differential/CMakeLists.txt
View File

@ -54,7 +54,7 @@ px4_add_module(
DifferentialManualMode
DifferentialOffboardMode
px4_work_queue
rover_control
surface_vehicle_control
pure_pursuit
MODULE_CONFIG
module.yaml

2
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp
View File

@ -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{};

2
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

2
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp
View File

@ -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;

2
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <math.h>

5
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp
View File

@ -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(

2
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes

2
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
View File

@ -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);

2
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

3
src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp
View File

@ -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;

2
src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/slew_rate/SlewRate.hpp>

2
src/modules/rover_mecanum/CMakeLists.txt
View File

@ -54,7 +54,7 @@ px4_add_module(
MecanumManualMode
MecanumOffboardMode
px4_work_queue
rover_control
surface_vehicle_control
pure_pursuit
MODULE_CONFIG
module.yaml

4
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp
View File

@ -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{};

2
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

2
src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp
View File

@ -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;

2
src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <math.h>

5
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp
View File

@ -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(

2
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp
View File

@ -37,7 +37,7 @@
#include <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes

2
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp
View File

@ -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);

2
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>

6
src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp
View File

@ -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);

2
src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/events.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/slew_rate/SlewRate.hpp>