Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:52:23 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

differential: restructure and update module (#23430)

Browse Source 
* differential: rename module

* differential: restructure and update module
This commit is contained in:
chfriedrich98
2024-08-07 09:53:37 +02:00
committed by GitHub
parent bfcd4564a6
commit 33d99a13e8
40 changed files with 906 additions and 1244 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
+22 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
+6 -6
View File
@@ -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):
ROMFS/px4fmu_common/init.d/CMakeLists.txt
+1 -1
View File
@@ -78,7 +78,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
if(CONFIG_MODULES_ROVER_DIFFERENTIAL)
px4_add_romfs_files(
rc.rover_differential_apps
rc.rover_differential_defaults
ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
+1 -1
View File
@@ -143,7 +143,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
if(CONFIG_MODULES_ROVER_DIFFERENTIAL)
px4_add_romfs_files(
50003_aion_robotics_r1_rover
)
ROMFS/px4fmu_common/init.d/rc.rover_differential_apps
+1 -1
View File
@@ -2,7 +2,7 @@
# Standard apps for a differential drive rover.
# Start rover differential drive controller.
differential_drive start
rover_differential start
# Start Land Detector.
land_detector start rover
ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults
+5 -6
View File
@@ -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
boards/px4/fmu-v5/rover.px4board
+2 -1
View File
@@ -17,5 +17,6 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/fmu-v5x/default.px4board
-1
View File
@@ -54,7 +54,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
boards/px4/fmu-v5x/rover.px4board
+2
View File
@@ -14,4 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/fmu-v6c/rover.px4board
+2 -1
View File
@@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/fmu-v6u/rover.px4board
+2 -1
View File
@@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/fmu-v6x/rover.px4board
+2 -1
View File
@@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/fmu-v6xrt/rover.px4board
+2 -1
View File
@@ -14,5 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_DRIVERS_ROBOCLAW=y
boards/px4/sitl/default.px4board
+1 -1
View File
@@ -13,7 +13,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y
CONFIG_EKF2_VERBOSE_STATUS=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
@@ -47,6 +46,7 @@ CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_REPLAY=y
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_COMMON_SIMULATION=y
msg/CMakeLists.txt
+2 -1
View File
@@ -71,7 +71,6 @@ set(msg_files
DebugKeyValue.msg
DebugValue.msg
DebugVect.msg
DifferentialDriveSetpoint.msg
DifferentialPressure.msg
DistanceSensor.msg
Ekf2Timestamps.msg
@@ -181,6 +180,8 @@ set(msg_files
RegisterExtComponentRequest.msg
RoverAckermannGuidanceStatus.msg
RoverAckermannStatus.msg
RoverDifferentialGuidanceStatus.msg
RoverDifferentialStatus.msg
Rpm.msg
RtlStatus.msg
RtlTimeEstimate.msg
msg/DifferentialDriveSetpoint.msg
-8
View File
@@ -1,8 +0,0 @@
uint64 timestamp # time since system start (microseconds)
float32 speed # [m/s] collective roll-off speed in body x-axis
bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
float32 yaw_rate # [rad/s] yaw rate
bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
# TOPICS differential_drive_setpoint differential_drive_control_output
msg/RoverDifferentialGuidanceStatus.msg
+10
View File
@@ -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
msg/RoverDifferentialStatus.msg
+8
View File
@@ -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
src/modules/control_allocator/ControlAllocator.cpp
+1 -1
View File
@@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
break;
case EffectivenessSource::ROVER_DIFFERENTIAL:
// differential_drive_control does allocation and publishes directly to actuator_motors topic
// rover_differential_control does allocation and publishes directly to actuator_motors topic
break;
case EffectivenessSource::FIXED_WING:
src/modules/differential_drive/DifferentialDrive.cpp
-191
View File
@@ -1,191 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "DifferentialDrive.hpp"
DifferentialDrive::DifferentialDrive() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
updateParams();
}
bool DifferentialDrive::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
void DifferentialDrive::updateParams()
{
ModuleParams::updateParams();
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
_max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get();
_differential_drive_guidance.setMaxSpeed(_max_speed);
_differential_drive_kinematics.setMaxSpeed(_max_speed);
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
_differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
}
void DifferentialDrive::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
hrt_abstime now = hrt_absolute_time();
const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
_time_stamp_last = now;
if (_parameter_update_sub.updated()) {
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
if (_vehicle_control_mode_sub.updated()) {
vehicle_control_mode_s vehicle_control_mode{};
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
}
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
if (_vehicle_status_sub.copy(&vehicle_status)) {
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
_differential_drive_kinematics.setArmed(spooled_up);
_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
}
}
if (_manual_driving) {
// Manual mode
// directly produce setpoints from the manual control setpoint (joystick)
if (_manual_control_setpoint_sub.updated()) {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
differential_drive_setpoint_s setpoint{};
setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
// if acro mode, we activate the yaw rate control
if (_acro_driving) {
setpoint.closed_loop_speed_control = false;
setpoint.closed_loop_yaw_rate_control = true;
} else {
setpoint.closed_loop_speed_control = false;
setpoint.closed_loop_yaw_rate_control = false;
}
setpoint.timestamp = now;
_differential_drive_setpoint_pub.publish(setpoint);
}
}
} else if (_mission_driving) {
// Mission mode
// directly receive setpoints from the guidance library
_differential_drive_guidance.computeGuidance(
_differential_drive_control.getVehicleYaw(),
_differential_drive_control.getVehicleBodyYawRate(),
dt
);
}
_differential_drive_control.control(dt);
_differential_drive_kinematics.allocate();
}
int DifferentialDrive::task_spawn(int argc, char *argv[])
{
DifferentialDrive *instance = new DifferentialDrive();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int DifferentialDrive::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int DifferentialDrive::print_usage(const char *reason)
{
if (reason) {
PX4_ERR("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Rover Differential Drive controller.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("differential_drive", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
{
return DifferentialDrive::main(argc, argv);
}
src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(DifferentialDriveControl
DifferentialDriveControl.cpp
)
target_link_libraries(DifferentialDriveControl PUBLIC pid)
target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp
-109
View File
@@ -1,109 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "DifferentialDriveControl.hpp"
using namespace matrix;
DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
{
pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
}
void DifferentialDriveControl::updateParams()
{
ModuleParams::updateParams();
pid_set_parameters(&_pid_angular_velocity,
_param_rdd_p_gain_angular_velocity.get(), // Proportional gain
_param_rdd_i_gain_angular_velocity.get(), // Integral gain
0, // Derivative gain
20, // Integral limit
200); // Output limit
pid_set_parameters(&_pid_speed,
_param_rdd_p_gain_speed.get(), // Proportional gain
_param_rdd_i_gain_speed.get(), // Integral gain
0, // Derivative gain
2, // Integral limit
200); // Output limit
}
void DifferentialDriveControl::control(float dt)
{
if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
}
}
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
}
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = velocity_in_body_frame(0);
}
}
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
// PID to reach setpoint using control_output
differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
if (_differential_drive_setpoint.closed_loop_speed_control) {
differential_drive_control_output.speed +=
pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt);
}
if (_differential_drive_setpoint.closed_loop_yaw_rate_control) {
differential_drive_control_output.yaw_rate +=
pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
}
differential_drive_control_output.timestamp = hrt_absolute_time();
_differential_drive_control_output_pub.publish(differential_drive_control_output);
}
src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp
-91
View File
@@ -1,91 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DifferentialDriveControl.hpp
*
* Controller for heading rate and forward speed.
*/
#pragma once
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_drive_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
class DifferentialDriveControl : public ModuleParams
{
public:
DifferentialDriveControl(ModuleParams *parent);
~DifferentialDriveControl() = default;
void control(float dt);
float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
float getVehicleYaw() const { return _vehicle_yaw; }
protected:
void updateParams() override;
private:
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
differential_drive_setpoint_s _differential_drive_setpoint{};
matrix::Quatf _vehicle_attitude_quaternion{};
float _vehicle_yaw{0.f};
// States
float _vehicle_body_yaw_rate{0.f};
float _vehicle_forward_speed{0.f};
PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
PID_t _pid_speed; ///< The PID controller for velocity.
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
)
};
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
-138
View File
@@ -1,138 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "DifferentialDriveGuidance.hpp"
#include <mathlib/math/Limits.hpp>
using namespace matrix;
DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
}
void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
{
if (_position_setpoint_triplet_sub.updated()) {
_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
}
if (_vehicle_global_position_sub.updated()) {
_vehicle_global_position_sub.copy(&_vehicle_global_position);
}
matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
current_waypoint(0),
current_waypoint(1));
float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
current_waypoint(1));
float heading_error = matrix::wrap_pi(desired_heading - yaw);
if (_current_waypoint != current_waypoint) {
_currentState = GuidanceState::TURNING;
}
// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
_currentState = GuidanceState::GOAL_REACHED;
}
float desired_speed = 0.f;
switch (_currentState) {
case GuidanceState::TURNING:
desired_speed = 0.f;
if (fabsf(heading_error) < 0.05f) {
_currentState = GuidanceState::DRIVING;
}
break;
case GuidanceState::DRIVING: {
const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
_param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
_forwards_velocity_smoothing.updateDurations(max_velocity);
_forwards_velocity_smoothing.updateTraj(dt);
desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
break;
}
case GuidanceState::GOAL_REACHED:
// temporary till I find a better way to stop the vehicle
desired_speed = 0.f;
heading_error = 0.f;
angular_velocity = 0.f;
_desired_angular_velocity = 0.f;
break;
}
// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
dt) + heading_error;
differential_drive_setpoint_s output{};
output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
output.timestamp = hrt_absolute_time();
_differential_drive_setpoint_pub.publish(output);
_current_waypoint = current_waypoint;
}
void DifferentialDriveGuidance::updateParams()
{
ModuleParams::updateParams();
pid_set_parameters(&_heading_p_controller,
_param_rdd_p_gain_heading.get(), // Proportional gain
0, // Integral gain
0, // Derivative gain
0, // Integral limit
200); // Output limit
_forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get());
_forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get());
_forwards_velocity_smoothing.setMaxVel(_max_speed);
}
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
-140
View File
@@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <math.h>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/motion_planning/VelocitySmoothing.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_drive_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <lib/pid/pid.h>
/**
* @brief Enum class for the different states of guidance.
*/
enum class GuidanceState {
TURNING, ///< The vehicle is currently turning.
DRIVING, ///< The vehicle is currently driving straight.
GOAL_REACHED ///< The vehicle has reached its goal.
};
/**
* @brief Class for differential drive guidance.
*/
class DifferentialDriveGuidance : public ModuleParams
{
public:
/**
* @brief Constructor for DifferentialDriveGuidance.
* @param parent The parent ModuleParams object.
*/
DifferentialDriveGuidance(ModuleParams *parent);
~DifferentialDriveGuidance() = default;
/**
* @brief Compute guidance for the vehicle.
* @param global_pos The global position of the vehicle in degrees.
* @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
* @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
* @param vehicle_yaw The yaw orientation of the vehicle in radians.
* @param body_velocity The velocity of the vehicle in m/s.
* @param angular_velocity The angular velocity of the vehicle in rad/s.
* @param dt The time step in seconds.
*/
void computeGuidance(float yaw, float angular_velocity, float dt);
/**
* @brief Set the maximum speed for the vehicle.
* @param max_speed The maximum speed in m/s.
* @return The set maximum speed in m/s.
*/
float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
/**
* @brief Set the maximum angular velocity for the vehicle.
* @param max_angular_velocity The maximum angular velocity in rad/s.
* @return The set maximum angular velocity in rad/s.
*/
float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
private:
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
position_setpoint_triplet_s _position_setpoint_triplet{};
vehicle_global_position_s _vehicle_global_position{};
GuidanceState _currentState; ///< The current state of guidance.
float _desired_angular_velocity; ///< The desired angular velocity.
float _max_speed; ///< The maximum speed.
float _max_angular_velocity; ///< The maximum angular velocity.
matrix::Vector2d _current_waypoint; ///< The current waypoint.
VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
PositionSmoothing _position_smoothing; ///< The position smoothing.
PID_t _heading_p_controller; ///< The PID controller for yaw rate.
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
)
};
src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt
-40
View File
@@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(DifferentialDriveKinematics
DifferentialDriveKinematics.cpp
)
target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
-97
View File
@@ -1,97 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "DifferentialDriveKinematics.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
using namespace time_literals;
DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
{}
void DifferentialDriveKinematics::allocate()
{
hrt_abstime now = hrt_absolute_time();
if (_differential_drive_control_output_sub.updated()) {
_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
}
const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now;
Vector2f wheel_speeds =
computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate);
if (!_armed || setpoint_timeout) {
wheel_speeds = {}; // stop
}
wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
wheel_speeds.copyTo(actuator_motors.control);
actuator_motors.timestamp = now;
_actuator_motors_pub.publish(actuator_motors);
}
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
{
if (_max_speed < FLT_EPSILON) {
return Vector2f();
}
linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
// Compute an initial gain
float gain = 1.0f;
if (combined_velocity > _max_speed) {
float excess_velocity = fabsf(combined_velocity - _max_speed);
const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
}
// Apply the gain
linear_velocity_x *= gain;
// Calculate the left and right wheel speeds
return Vector2f(linear_velocity_x - rotational_velocity,
linear_velocity_x + rotational_velocity) / _max_speed;
}
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
-103
View File
@@ -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 <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/differential_drive_setpoint.h>
/**
* @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
*
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
* given linear velocity and yaw rate.
*/
class DifferentialDriveKinematics : public ModuleParams
{
public:
DifferentialDriveKinematics(ModuleParams *parent);
~DifferentialDriveKinematics() = default;
/**
* @brief Sets the wheel base of the robot.
*
* @param wheel_base The distance between the centers of the wheels.
*/
void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
/**
* @brief Sets the maximum speed of the robot.
*
* @param max_speed The maximum speed of the robot.
*/
void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
/**
* @brief Sets the maximum angular speed of the robot.
*
* @param max_angular_speed The maximum angular speed of the robot.
*/
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
void setArmed(const bool armed) { _armed = armed; };
void allocate();
/**
* @brief Computes the inverse kinematics for differential drive.
*
* @param linear_velocity_x Linear velocity along the x-axis.
* @param yaw_rate Yaw rate of the robot.
* @return matrix::Vector2f Motor velocities for the right and left motors.
*/
matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;
private:
uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
differential_drive_setpoint_s _differential_drive_control_output{};
bool _armed = false;
float _wheel_base{0.f};
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
-168
View File
@@ -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 <gtest/gtest.h>
#include "DifferentialDriveKinematics.hpp"
#include <mathlib/math/Functions.hpp>
using namespace matrix;
class DifferentialDriveKinematicsTest : public ::testing::Test
{
public:
DifferentialDriveKinematics kinematics{nullptr};
};
TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with zero linear velocity and zero yaw rate (stationary vehicle)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
}
TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase)
{
kinematics.setWheelBase(0.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with invalid parameters (zero wheel base and wheel radius)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
}
TEST_F(DifferentialDriveKinematicsTest, UnitCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with unit values for linear velocity and yaw rate
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
}
TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
}
TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Negative linear velocity for backward motion and positive yaw rate for turning right
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
}
TEST_F(DifferentialDriveKinematicsTest, RandomCase)
{
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Negative linear velocity for backward motion and positive yaw rate for turning right
EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
}
TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test rotating in place (zero linear velocity, non-zero yaw rate)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
}
TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test moving straight (non-zero linear velocity, zero yaw rate)
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
}
TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase)
{
kinematics.setWheelBase(FLT_MIN);
kinematics.setMaxSpeed(FLT_MIN);
kinematics.setMaxAngularVelocity(FLT_MIN);
// Test with minimum possible input values
EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
}
TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
}
TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
{
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
}
TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase)
{
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
}
src/modules/differential_drive/Kconfig
-6
View File
@@ -1,6 +0,0 @@
menuconfig MODULES_DIFFERENTIAL_DRIVE
bool "differential_drive"
default n
depends on MODULES_CONTROL_ALLOCATOR
---help---
Enable support for control of differential drive rovers
src/modules/logger/logged_topics.cpp
+2 -2
View File
@@ -58,8 +58,6 @@ void LoggedTopics::add_default_topics()
add_topic("commander_state");
add_topic("config_overrides");
add_topic("cpuload");
add_optional_topic("differential_drive_control_output", 100);
add_optional_topic("differential_drive_setpoint", 100);
add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position");
add_optional_topic("external_ins_local_position");
@@ -105,6 +103,8 @@ void LoggedTopics::add_default_topics()
add_topic("radio_status");
add_optional_topic("rover_ackermann_guidance_status", 100);
add_optional_topic("rover_ackermann_status", 100);
add_optional_topic("rover_differential_guidance_status", 100);
add_optional_topic("rover_differential_status", 100);
add_topic("rtl_time_estimate", 1000);
add_topic("rtl_status", 2000);
add_optional_topic("sensor_airflow", 100);
src/modules/differential_drive/CMakeLists.txt → src/modules/rover_differential/CMakeLists.txt
+7 -10
View File
@@ -31,22 +31,19 @@
#
############################################################################
add_subdirectory(DifferentialDriveControl)
add_subdirectory(DifferentialDriveGuidance)
add_subdirectory(DifferentialDriveKinematics)
add_subdirectory(RoverDifferentialGuidance)
px4_add_module(
MODULE modules__differential_drive
MAIN differential_drive
MODULE modules__rover_differential
MAIN rover_differential
SRCS
DifferentialDrive.cpp
DifferentialDrive.hpp
RoverDifferential.cpp
RoverDifferential.hpp
DEPENDS
DifferentialDriveControl
DifferentialDriveGuidance
DifferentialDriveKinematics
RoverDifferentialGuidance
px4_work_queue
modules__control_allocator # for parameter CA_R_REV
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_differential/Kconfig
+6
View File
@@ -0,0 +1,6 @@
menuconfig MODULES_ROVER_DIFFERENTIAL
bool "rover_differential"
default n
depends on MODULES_CONTROL_ALLOCATOR
---help---
Enable support for control of differential rovers
src/modules/rover_differential/RoverDifferential.cpp
+256
View File
@@ -0,0 +1,256 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "RoverDifferential.hpp"
using namespace matrix;
using namespace time_literals;
RoverDifferential::RoverDifferential() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
updateParams();
_rover_differential_status_pub.advertise();
pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
}
bool RoverDifferential::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
void RoverDifferential::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
pid_set_parameters(&_pid_yaw_rate,
_param_rd_p_gain_yaw_rate.get(), // Proportional gain
_param_rd_i_gain_yaw_rate.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
}
void RoverDifferential::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// uORB subscriber updates
if (_parameter_update_sub.updated()) {
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
}
if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
}
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = velocity_in_body_frame(0);
}
// Navigation modes
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_differential_setpoint.throttle = manual_control_setpoint.throttle;
_differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
}
_differential_setpoint.closed_loop_yaw_rate = false;
} break;
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_differential_setpoint.throttle = manual_control_setpoint.throttle;
_differential_setpoint.yaw_rate = math::interpolate<float>(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<float>(speed_diff, -_param_rd_max_speed.get(),
_param_rd_max_speed.get(), -1.f, 1.f);
speed_diff_normalized = math::constrain(speed_diff_normalized +
pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
-1.f, 1.f); // Feedback
}
} else {
pid_reset_integral(&_pid_yaw_rate);
}
// Publish rover differential status (logging)
rover_differential_status_s rover_differential_status{};
rover_differential_status.timestamp = _timestamp;
rover_differential_status.actual_speed = _vehicle_forward_speed;
rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate;
rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate;
rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
_rover_differential_status_pub.publish(rover_differential_status);
// Publish to motors
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff)
{
float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff);
if (combined_velocity > 1.0f) { // Prioritize yaw rate
float excess_velocity = fabsf(combined_velocity - 1.0f);
forward_speed -= sign(forward_speed) * excess_velocity;
}
// Calculate the left and right wheel speeds
return Vector2f(forward_speed - speed_diff,
forward_speed + speed_diff);
}
int RoverDifferential::task_spawn(int argc, char *argv[])
{
RoverDifferential *instance = new RoverDifferential();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int RoverDifferential::custom_command(int argc, char *argv[])
{
return print_usage("unk_timestampn command");
}
int RoverDifferential::print_usage(const char *reason)
{
if (reason) {
PX4_ERR("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Rover Differential controller.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("rover_differential", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int rover_differential_main(int argc, char *argv[])
{
return RoverDifferential::main(argc, argv);
}
src/modules/differential_drive/DifferentialDrive.hpp → src/modules/rover_differential/RoverDifferential.hpp
+57 -26
View File
@@ -33,31 +33,41 @@
#pragma once
// PX4 includes
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_drive_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/rover_differential_status.h>
#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"
// Standard libraries
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
// Local includes
#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp"
using namespace time_literals;
class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
class RoverDifferential : public ModuleBase<RoverDifferential>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
DifferentialDrive();
~DifferentialDrive() override = default;
RoverDifferential();
~RoverDifferential() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -70,36 +80,57 @@ public:
bool init();
/**
* @brief Computes motor commands for differential drive.
*
* @param forward_speed Linear velocity along the x-axis.
* @param speed_diff Speed difference between left and right wheels.
* @return matrix::Vector2f Motor velocities for the right and left motors.
*/
matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff);
protected:
void updateParams() override;
private:
void Run() override;
// uORB Subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
bool _manual_driving = false;
bool _mission_driving = false;
bool _acro_driving = false;
hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
// uORB Publications
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_differential_status_s> _rover_differential_status_pub{ORB_ID(rover_differential_status)};
DifferentialDriveGuidance _differential_drive_guidance{this};
DifferentialDriveControl _differential_drive_control{this};
DifferentialDriveKinematics _differential_drive_kinematics{this};
// Instances
RoverDifferentialGuidance _rover_differential_guidance{this};
float _max_speed{0.f};
float _max_angular_velocity{0.f};
// Variables
float _vehicle_body_yaw_rate{0.f};
float _vehicle_forward_speed{0.f};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
int _nav_state{0};
matrix::Quatf _vehicle_attitude_quaternion{};
hrt_abstime _timestamp{0};
PID_t _pid_yaw_rate; // The PID controller for yaw rate
RoverDifferentialGuidance::differential_setpoint _differential_setpoint;
// Constants
static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_p_gain_yaw_rate,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_i_gain_yaw_rate,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt → src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt
+4 -3
View File
@@ -31,8 +31,9 @@
#
############################################################################
px4_add_library(DifferentialDriveGuidance
DifferentialDriveGuidance.cpp
px4_add_library(RoverDifferentialGuidance
RoverDifferentialGuidance.cpp
)
target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(RoverDifferentialGuidance PUBLIC pid)
target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+247
View File
@@ -0,0 +1,247 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "RoverDifferentialGuidance.hpp"
#include <mathlib/math/Limits.hpp>
using namespace matrix;
using namespace time_literals;
RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
_rover_differential_guidance_status_pub.advertise();
pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
}
void RoverDifferentialGuidance::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
pid_set_parameters(&_pid_heading,
_param_rd_p_gain_heading.get(), // Proportional gain
_param_rd_i_gain_heading.get(), // Integral gain
0.f, // Derivative gain
_max_yaw_rate, // Integral limit
_max_yaw_rate); // Output limit
pid_set_parameters(&_pid_throttle,
_param_rd_p_gain_speed.get(), // Proportional gain
_param_rd_i_gain_speed.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // Output limit
}
RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw,
const float actual_speed, const int nav_state)
{
// Initializations
bool mission_finished{false};
float desired_speed{0.f};
float desired_yaw_rate{0.f};
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// uORB subscriber updates
if (_vehicle_global_position_sub.updated()) {
vehicle_global_position_s vehicle_global_position{};
_vehicle_global_position_sub.copy(&vehicle_global_position);
_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
}
if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
}
_curr_pos_ned = Vector2f(local_position.x, local_position.y);
}
if (_position_setpoint_triplet_sub.updated()) {
updateWaypoints();
}
if (_mission_result_sub.updated()) {
mission_result_s mission_result{};
_mission_result_sub.copy(&mission_result);
mission_finished = mission_result.finished;
}
if (_home_position_sub.updated()) {
home_position_s home_position{};
_home_position_sub.copy(&home_position);
_home_position = Vector2d(home_position.lat, home_position.lon);
}
const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
math::max(actual_speed, 0.f));
const float heading_error = matrix::wrap_pi(desired_heading - yaw);
const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
_curr_wp(0),
_curr_wp(1));
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
&& distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch
mission_finished = true;
}
// State machine
if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) {
if (_currentState == GuidanceState::STOPPED) {
_currentState = GuidanceState::DRIVING;
}
if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
pid_reset_integral(&_pid_heading);
_currentState = GuidanceState::SPOT_TURNING;
} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
pid_reset_integral(&_pid_heading);
_currentState = GuidanceState::DRIVING;
}
} else { // Mission finished or delay command
_currentState = GuidanceState::STOPPED;
}
// Guidance logic
switch (_currentState) {
case GuidanceState::DRIVING: {
desired_speed = _param_rd_miss_spd_def.get();
if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
_param_rd_max_accel.get(), distance_to_next_wp, 0.0f);
desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
}
desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt);
} break;
case GuidanceState::SPOT_TURNING:
if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop
desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot
}
break;
case GuidanceState::STOPPED:
default:
desired_speed = 0.f;
desired_yaw_rate = 0.f;
break;
}
// Closed loop speed control
float throttle{0.f};
if (fabsf(desired_speed) < FLT_EPSILON) {
pid_reset_integral(&_pid_throttle);
} else {
throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0,
dt);
if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
throttle += math::interpolate<float>(desired_speed,
0.f, _param_rd_max_speed.get(),
0.f, 1.f);
}
}
// Publish differential controller status (logging)
_rover_differential_guidance_status.timestamp = _timestamp;
_rover_differential_guidance_status.desired_speed = desired_speed;
_rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral;
_rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
_rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral;
_rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error;
_rover_differential_guidance_status.state_machine = (uint8_t) _currentState;
_rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status);
// Return setpoints
differential_setpoint differential_setpoint_temp;
differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f);
differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate,
_max_yaw_rate);
differential_setpoint_temp.closed_loop_yaw_rate = true;
return differential_setpoint_temp;
}
void RoverDifferentialGuidance::updateWaypoints()
{
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
// Global waypoint coordinates
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
} else {
_curr_wp = Vector2d(0, 0);
}
if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
&& PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
} else {
_prev_wp = _curr_pos;
}
if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
&& PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
} else {
_next_wp = _home_position;
}
// NED waypoint coordinates
_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
}
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
// PX4 includes
#include <px4_platform_common/module_params.h>
#include <lib/pure_pursuit/PurePursuit.hpp>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rover_differential_guidance_status.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <math.h>
using namespace matrix;
/**
* @brief Enum class for the different states of guidance.
*/
enum class GuidanceState {
SPOT_TURNING, // The vehicle is currently turning on the spot.
DRIVING, // The vehicle is currently driving.
STOPPED // The vehicle is stopped.
};
/**
* @brief Class for differential rover guidance.
*/
class RoverDifferentialGuidance : public ModuleParams
{
public:
/**
* @brief Constructor for RoverDifferentialGuidance.
* @param parent The parent ModuleParams object.
*/
RoverDifferentialGuidance(ModuleParams *parent);
~RoverDifferentialGuidance() = default;
struct differential_setpoint {
float throttle{0.f};
float yaw_rate{0.f};
bool closed_loop_yaw_rate{false};
};
/**
* @brief Compute guidance for the vehicle.
* @param yaw The yaw orientation of the vehicle in radians.
* @param actual_speed The velocity of the vehicle in m/s.
* @param dt The time step in seconds.
* @param nav_state Navigation state of the rover.
*/
RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed,
int nav_state);
/**
* @brief Update global/ned waypoint coordinates
*/
void updateWaypoints();
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
private:
// uORB subscriptions
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
// uORB publications
uORB::Publication<rover_differential_guidance_status_s> _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<px4::params::RD_HEADING_P>) _param_rd_p_gain_heading,
(ParamFloat<px4::params::RD_HEADING_I>) _param_rd_i_gain_heading,
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
)
};
src/modules/differential_drive/module.yaml → src/modules/rover_differential/module.yaml
+94 -44
View File
@@ -1,11 +1,12 @@
module_name: Differential Drive
module_name: Rover Differential
parameters:
- group: Rover Differential Drive
- group: Rover Differential
definitions:
RDD_WHEEL_BASE:
RD_WHEEL_TRACK:
description:
short: Wheel base
short: Wheel track
long: Distance from the center of the right wheel to the center of the left wheel
type: float
unit: m
@@ -14,46 +15,21 @@ parameters:
increment: 0.001
decimal: 3
default: 0.5
RDD_WHEEL_RADIUS:
RD_MAN_YAW_SCALE:
description:
short: Wheel radius
long: Size of the wheel, half the diameter of the wheel
short: Manual yaw rate scale
long: |
In manual mode the setpoint for the yaw rate received from the rc remote
is scaled by this value.
type: float
unit: m
min: 0.001
max: 100
increment: 0.001
decimal: 3
default: 0.1
RDD_SPEED_SCALE:
description:
short: Manual speed scale
type: float
min: 0
min: 0.01
max: 1
increment: 0.01
decimal: 2
default: 1
RDD_ANG_SCALE:
description:
short: Manual angular velocity scale
type: float
min: 0
max: 1
increment: 0.01
decimal: 2
default: 1
RDD_WHEEL_SPEED:
description:
short: Maximum wheel speed
type: float
unit: rad/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0.3
RDD_P_HEADING:
RD_HEADING_P:
description:
short: Proportional gain for heading controller
type: float
@@ -62,7 +38,18 @@ parameters:
increment: 0.01
decimal: 2
default: 1
RDD_P_SPEED:
RD_HEADING_I:
description:
short: Integral gain for heading controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0.1
RD_SPEED_P:
description:
short: Proportional gain for speed controller
type: float
@@ -71,7 +58,8 @@ parameters:
increment: 0.01
decimal: 2
default: 1
RDD_I_SPEED:
RD_SPEED_I:
description:
short: Integral gain for ground speed controller
type: float
@@ -80,7 +68,8 @@ parameters:
increment: 0.01
decimal: 2
default: 0
RDD_P_ANG_VEL:
RD_YAW_RATE_P:
description:
short: Proportional gain for angular velocity controller
type: float
@@ -89,7 +78,8 @@ parameters:
increment: 0.01
decimal: 2
default: 1
RDD_I_ANG_VEL:
RD_YAW_RATE_I:
description:
short: Integral gain for angular velocity controller
type: float
@@ -98,7 +88,8 @@ parameters:
increment: 0.01
decimal: 2
default: 0
RDD_MAX_JERK:
RD_MAX_JERK:
description:
short: Maximum jerk
long: Limit for forwards acc/deceleration change.
@@ -109,7 +100,8 @@ parameters:
increment: 0.01
decimal: 2
default: 0.5
RDD_MAX_ACCEL:
RD_MAX_ACCEL:
description:
short: Maximum acceleration
long: Maximum acceleration is used to limit the acceleration of the rover
@@ -120,3 +112,61 @@ parameters:
increment: 0.01
decimal: 2
default: 0.5
RD_MAX_SPEED:
description:
short: Maximum speed the rover can drive
long: This parameter is used to map desired speeds to normalized motor commands.
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 7
RD_MAX_YAW_RATE:
description:
short: Maximum allowed yaw rate for the rover.
long: |
This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
type: float
unit: deg/s
min: 0.01
max: 1000
increment: 0.01
decimal: 2
default: 90
RD_MISS_SPD_DEF:
description:
short: Default rover speed during a mission
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 1
RD_TRANS_TRN_DRV:
description:
short: Heading error threshhold to switch from spot turning to driving
type: float
unit: rad
min: 0.001
max: 180
increment: 0.01
decimal: 3
default: 0.0872665
RD_TRANS_DRV_TRN:
description:
short: Heading error threshhold to switch from driving to spot turning
type: float
unit: rad
min: 0.001
max: 180
increment: 0.01
decimal: 3
default: 0.174533
src/modules/uxrce_dds_client/dds_topics.yaml
-3
View File
@@ -130,9 +130,6 @@ subscriptions:
- topic: /fmu/in/vehicle_rates_setpoint
type: px4_msgs::msg::VehicleRatesSetpoint
- topic: /fmu/in/differential_drive_setpoint
type: px4_msgs::msg::DifferentialDriveSetpoint
- topic: /fmu/in/vehicle_visual_odometry
type: px4_msgs::msg::VehicleOdometry