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

differential: update module (#23629)

Browse Source 
Improve the slow down effect and add support for speed change in mission mode.
Seperate code related to turning setpoints into motor commands into its own folder and refactor code.
This commit is contained in:
chfriedrich98 2024-08-29 15:27:08 +02:00 committed by GitHub
parent c86d44f831
commit f8188f0981
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
18 changed files with 615 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
View File

@ -14,15 +14,17 @@ 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_YAW_RATE_P 5
param set-default RD_YAW_RATE_I 0
param set-default RD_YAW_P 5
param set-default RD_YAW_I 0
param set-default RD_SPEED_P 1
param set-default RD_SPEED_I 0
param set-default RD_MAX_YAW_RATE 180
param set-default RD_MISS_SPD_DEF 7
param set-default RD_MISS_SPD_DEF 5
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533

4
ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
View File

@ -27,8 +27,8 @@ param set-default RD_YAW_RATE_P 5
param set-default RD_MAX_ACCEL 1
param set-default RD_MAX_JERK 3
param set-default RD_MAX_SPEED 8
param set-default RD_HEADING_P 5
param set-default RD_HEADING_I 0.1
param set-default RD_YAW_P 5
param set-default RD_YAW_I 0.1
param set-default RD_MAX_YAW_RATE 30
param set-default RD_MISS_SPD_DEF 8
param set-default RD_TRANS_DRV_TRN 0.349066

22
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
View File

@ -20,3 +20,25 @@ param set-default RBCLW_ADDRESS 128
param set-default RBCLW_FUNC1 101
param set-default RBCLW_FUNC2 102
param set-default RBCLW_REV 1 # reverse right wheels
# Rover parameters
param set-default RD_WHEEL_TRACK 0.3
param set-default RD_MAN_YAW_SCALE 1
param set-default RD_MAX_ACCEL 5
param set-default RD_MAX_JERK 50
param set-default RD_MAX_SPEED 2
param set-default RD_YAW_RATE_P 0.1
param set-default RD_YAW_RATE_I 0
param set-default RD_YAW_P 5
param set-default RD_YAW_I 0
param set-default RD_SPEED_P 0.5
param set-default RD_SPEED_I 0.1
param set-default RD_MAX_YAW_RATE 300
param set-default RD_MISS_SPD_DEF 1.8
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 10
param set-default PP_LOOKAHD_MIN 1
param set-default PP_LOOKAHD_GAIN 1

1
msg/CMakeLists.txt
View File

@ -182,6 +182,7 @@ set(msg_files
RoverAckermannGuidanceStatus.msg
RoverAckermannStatus.msg
RoverDifferentialGuidanceStatus.msg
RoverDifferentialSetpoint.msg
RoverDifferentialStatus.msg
Rpm.msg
RtlStatus.msg

3
msg/RoverDifferentialGuidanceStatus.msg
View File

@ -1,10 +1,7 @@
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

9
msg/RoverDifferentialSetpoint.msg Normal file
View File

@ -0,0 +1,9 @@
uint64 timestamp # time since system start (microseconds)
float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover
float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover
float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used)
float32 yaw_rate_setpoint_normalized # [-1, 1] Normalized yaw rate for the rover
float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover
# TOPICS rover_differential_setpoint

7
msg/RoverDifferentialStatus.msg
View File

@ -1,8 +1,11 @@
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_deg # [deg] Actual yaw of the rover
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
float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller
float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller
float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller
float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller
# TOPICS rover_differential_status

1
src/modules/logger/logged_topics.cpp
View File

@ -105,6 +105,7 @@ void LoggedTopics::add_default_topics()
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_setpoint", 100);
add_optional_topic("rover_differential_status", 100);
add_topic("rtl_time_estimate", 1000);
add_topic("rtl_status", 2000);

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

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
# 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
@ -32,6 +32,7 @@
############################################################################
add_subdirectory(RoverDifferentialGuidance)
add_subdirectory(RoverDifferentialControl)
px4_add_module(
MODULE modules__rover_differential
@ -41,8 +42,8 @@ px4_add_module(
RoverDifferential.hpp
DEPENDS
RoverDifferentialGuidance
RoverDifferentialControl
px4_work_queue
modules__control_allocator # for parameter CA_R_REV
pure_pursuit
MODULE_CONFIG
module.yaml

174
src/modules/rover_differential/RoverDifferential.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
* 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
@ -32,16 +32,13 @@
****************************************************************************/
#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);
_rover_differential_setpoint_pub.advertise();
}
bool RoverDifferential::init()
@ -53,16 +50,7 @@ bool RoverDifferential::init()
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()
@ -73,11 +61,67 @@ void RoverDifferential::Run()
return;
}
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
updateSubscriptions();
// Generate and publish attitude and velocity setpoints
hrt_abstime timestamp = hrt_absolute_time();
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)) {
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.timestamp = timestamp;
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
rover_differential_setpoint.yaw_rate_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
} break;
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.timestamp = timestamp;
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
} break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
break;
default: // Unimplemented nav states will stop the rover
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
rover_differential_setpoint.yaw_rate_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f;
rover_differential_setpoint.yaw_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
break;
}
_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
}
void RoverDifferential::updateSubscriptions()
{
// uORB subscriber updates
if (_parameter_update_sub.updated()) {
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
@ -93,7 +137,7 @@ void RoverDifferential::Run()
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];
_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
}
if (_vehicle_attitude_sub.updated()) {
@ -110,98 +154,6 @@ void RoverDifferential::Run()
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[])

43
src/modules/rover_differential/RoverDifferential.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
* 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
@ -42,23 +42,21 @@
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.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 <uORB/topics/rover_differential_setpoint.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
// Local includes
#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp"
#include "RoverDifferentialControl/RoverDifferentialControl.hpp"
using namespace time_literals;
@ -80,21 +78,17 @@ 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;
/**
* @brief Update uORB subscriptions.
*/
void updateSubscriptions();
// uORB Subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
@ -104,33 +98,22 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
// 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)};
uORB::Publication<rover_differential_setpoint_s> _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)};
// Instances
RoverDifferentialGuidance _rover_differential_guidance{this};
RoverDifferentialControl _rover_differential_control{this};
// Variables
float _vehicle_body_yaw_rate{0.f};
matrix::Quatf _vehicle_attitude_quaternion{};
float _vehicle_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::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
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate
)
};

39
src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(RoverDifferentialControl
RoverDifferentialControl.cpp
)
target_link_libraries(RoverDifferentialControl PUBLIC pid)
target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

176
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp Normal file
View File

@ -0,0 +1,176 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#include "RoverDifferentialControl.hpp"
#include <mathlib/math/Limits.hpp>
using namespace matrix;
using namespace time_literals;
RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
_rover_differential_status_pub.advertise();
pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
}
void RoverDifferentialControl::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_yaw_rate_p.get(), // Proportional gain
_param_rd_yaw_rate_i.get(), // Integral gain
0.f, // Derivative gain
1.f, // Integral limit
1.f); // 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
pid_set_parameters(&_pid_yaw,
_param_rd_p_gain_yaw.get(), // Proportional gain
_param_rd_i_gain_yaw.get(), // Integral gain
0.f, // Derivative gain
_max_yaw_rate, // Integral limit
_max_yaw_rate); // Output limit
}
void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
const float vehicle_forward_speed)
{
// Timestamps
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// Update differential setpoint
_rover_differential_setpoint_sub.update(&_rover_differential_setpoint);
// Closed loop yaw control (Overrides yaw rate setpoint)
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) {
_rover_differential_setpoint.yaw_rate_setpoint = 0.f;
pid_reset_integral(&_pid_yaw);
} else {
const float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
}
}
// Yaw rate control
float speed_diff_normalized{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) {
speed_diff_normalized = 0.f;
pid_reset_integral(&_pid_yaw_rate);
} else {
const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _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, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
-1.f, 1.f); // Feedback
}
} else { // Use normalized setpoint
speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
}
// Speed control
float throttle{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) {
pid_reset_integral(&_pid_throttle);
} else {
throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
dt);
if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
throttle += math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
0.f, _param_rd_max_speed.get(),
0.f, 1.f);
}
}
} else { // Use normalized setpoint
throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
}
// 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.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw;
rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_differential_setpoint.yaw_rate_setpoint;
rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate;
rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
rover_differential_status.pid_yaw_integral = _pid_yaw.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();
computeInverseKinematics(throttle, speed_diff_normalized).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized,
const float speed_diff_normalized)
{
float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized);
if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1
float excess = fabsf(max_motor_command - 1.0f);
forward_speed_normalized -= sign(forward_speed_normalized) * excess;
}
// Calculate the left and right wheel speeds
return Vector2f(forward_speed_normalized - speed_diff_normalized,
forward_speed_normalized + speed_diff_normalized);
}

121
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
// PX4 includes
#include <px4_platform_common/module_params.h>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_differential_setpoint.h>
#include <uORB/topics/rover_differential_status.h>
#include <uORB/topics/actuator_motors.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <math.h>
using namespace matrix;
/**
* @brief Class for differential rover guidance.
*/
class RoverDifferentialControl : public ModuleParams
{
public:
/**
* @brief Constructor for RoverDifferentialControl.
* @param parent The parent ModuleParams object.
*/
RoverDifferentialControl(ModuleParams *parent);
~RoverDifferentialControl() = default;
/**
* @brief Compute motor commands based on setpoints
*/
void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed);
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
private:
/**
* @brief Compute normalized motor commands based on normalized setpoints.
*
* @param forward_speed_normalized Normalized forward speed [-1, 1].
* @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
* @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1].
*/
matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized);
// uORB subscriptions
uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)};
// 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)};
// Variables
rover_differential_setpoint_s _rover_differential_setpoint{};
hrt_abstime _timestamp{0};
float _max_yaw_rate{0.f};
// Controllers
PID_t _pid_throttle; // The PID controller for the closed loop speed control
PID_t _pid_yaw; // The PID controller for the closed loop yaw control
PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
(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_YAW_P>) _param_rd_p_gain_yaw,
(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

1
src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt
View File

@ -35,5 +35,4 @@ px4_add_library(RoverDifferentialGuidance
RoverDifferentialGuidance.cpp
)
target_link_libraries(RoverDifferentialGuidance PUBLIC pid)
target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

231
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
* 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
@ -36,48 +36,107 @@
#include <mathlib/math/Limits.hpp>
using namespace matrix;
using namespace time_literals;
RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
_max_forward_speed = _param_rd_miss_spd_def.get();
_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)
void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_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;
updateSubscriptions();
// uORB subscriber updates
// Catch return to launch
const float distance_to_curr_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) {
_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
}
// State machine
float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
math::max(forward_speed, 0.f));
const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw);
if (!_mission_finished && distance_to_curr_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()) {
_currentState = GuidanceState::SPOT_TURNING;
} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
_currentState = GuidanceState::DRIVING;
}
} else { // Mission finished or delay command
_currentState = GuidanceState::STOPPED;
}
// Guidance logic
float desired_forward_speed{0.f};
switch (_currentState) {
case GuidanceState::DRIVING: {
// Calculate desired forward speed
desired_forward_speed = _max_forward_speed;
if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) {
if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
_param_rd_max_accel.get(), distance_to_curr_wp, 0.0f);
desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed);
}
}
} break;
case GuidanceState::SPOT_TURNING:
if (forward_speed > TURN_MAX_VELOCITY) {
desired_yaw = vehicle_yaw; // Wait for the rover to stop
}
break;
case GuidanceState::STOPPED:
default:
desired_yaw = vehicle_yaw;
break;
}
// Publish differential guidance status (logging)
hrt_abstime timestamp = hrt_absolute_time();
rover_differential_guidance_status_s rover_differential_guidance_status{};
rover_differential_guidance_status.timestamp = timestamp;
rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
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);
// Publish setpoints
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.timestamp = timestamp;
rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed;
rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_rate_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = desired_yaw;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
void RoverDifferentialGuidance::updateSubscriptions()
{
if (_vehicle_global_position_sub.updated()) {
vehicle_global_position_s vehicle_global_position{};
_vehicle_global_position_sub.copy(&vehicle_global_position);
@ -103,7 +162,7 @@ RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::comp
if (_mission_result_sub.updated()) {
mission_result_s mission_result{};
_mission_result_sub.copy(&mission_result);
mission_finished = mission_result.finished;
_mission_finished = mission_result.finished;
}
if (_home_position_sub.updated()) {
@ -111,103 +170,6 @@ RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::comp
_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()
@ -243,5 +205,20 @@ void RoverDifferentialGuidance::updateWaypoints()
// 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));
_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
// Distances
const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned;
const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned;
float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
_waypoint_transition_angle = acosf(cosin);
// Waypoint cruising speed
if (position_setpoint_triplet.current.cruising_speed > 0.f) {
_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
} else {
_max_forward_speed = _param_rd_miss_spd_def.get();
}
}

55
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
* 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
@ -47,9 +47,9 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rover_differential_guidance_status.h>
#include <uORB/topics/rover_differential_setpoint.h>
// Standard libraries
#include <lib/pid/pid.h>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
@ -80,26 +80,13 @@ public:
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.
* @brief Compute and publish attitude and velocity setpoints based on the mission plan.
* @param vehicle_yaw The yaw of the vehicle [rad].
* @param forward_speed The forward speed of the vehicle [m/s].
* @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();
void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state);
protected:
/**
@ -108,6 +95,16 @@ protected:
void updateParams() override;
private:
/**
* @brief Update uORB subscriptions.
*/
void updateSubscriptions();
/**
* @brief Update global/ned waypoint coordinates
*/
void updateWaypoints();
// uORB subscriptions
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
@ -116,16 +113,14 @@ private:
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
// uORB publications
uORB::Publication<rover_differential_setpoint_s> _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)};
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.
GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance.
PurePursuit _pure_pursuit{this}; // Pure pursuit library
hrt_abstime _timestamp{0};
float _max_yaw_rate{0.f};
bool _mission_finished{false};
// Waypoints
Vector2d _curr_pos{};
@ -135,27 +130,21 @@ private:
Vector2d _curr_wp{};
Vector2f _curr_wp_ned{};
Vector2d _next_wp{};
Vector2f _next_wp_ned{};
Vector2d _home_position{};
// Controllers
PID_t _pid_heading; // The PID controller for the heading
PID_t _pid_throttle; // The PID controller for velocity
float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s]
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
// 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

49
src/modules/rover_differential/module.yaml
View File

@ -23,70 +23,70 @@ parameters:
In manual mode the setpoint for the yaw rate received from the rc remote
is scaled by this value.
type: float
min: 0.01
min: 0.001
max: 1
increment: 0.01
decimal: 2
decimal: 3
default: 1
RD_HEADING_P:
RD_YAW_P:
description:
short: Proportional gain for heading controller
short: Proportional gain for closed loop yaw controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
decimal: 3
default: 1
RD_HEADING_I:
RD_YAW_I:
description:
short: Integral gain for heading controller
short: Integral gain for closed loop yaw controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0.1
decimal: 3
default: 0
RD_SPEED_P:
description:
short: Proportional gain for speed controller
short: Proportional gain for closed loop forward speed controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
decimal: 3
default: 1
RD_SPEED_I:
description:
short: Integral gain for ground speed controller
short: Integral gain for closed loop forward speed controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
decimal: 3
default: 0
RD_YAW_RATE_P:
description:
short: Proportional gain for angular velocity controller
short: Proportional gain for closed loop yaw rate controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
decimal: 3
default: 1
RD_YAW_RATE_I:
description:
short: Integral gain for angular velocity controller
short: Integral gain for closed loop yaw rate controller
type: float
min: 0
max: 100
increment: 0.01
decimal: 2
decimal: 3
default: 0
RD_MAX_JERK:
@ -123,11 +123,11 @@ parameters:
max: 100
increment: 0.01
decimal: 2
default: 7
default: 1
RD_MAX_YAW_RATE:
description:
short: Maximum allowed yaw rate for the rover.
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
@ -140,7 +140,7 @@ parameters:
RD_MISS_SPD_DEF:
description:
short: Default rover speed during a mission
short: Default forward speed for the rover during auto modes
type: float
unit: m/s
min: 0
@ -151,7 +151,7 @@ parameters:
RD_TRANS_TRN_DRV:
description:
short: Heading error threshhold to switch from spot turning to driving
short: Yaw error threshhold to switch from spot turning to driving
type: float
unit: rad
min: 0.001
@ -162,7 +162,12 @@ parameters:
RD_TRANS_DRV_TRN:
description:
short: Heading error threshhold to switch from driving to spot turning
short: Yaw error threshhold to switch from driving to spot turning
long: |
This threshold is used for the state machine to switch from driving to turning based on the
error between the desired and actual yaw. It is also used as the threshold whether the rover should come
to a smooth stop at the next waypoint. This slow down effect is active if the angle between the
line segments from prevWP-currWP and currWP-nextWP is smaller then 180 - RD_TRANS_DRV_TRN.
type: float
unit: rad
min: 0.001