mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 04:53:33 +08:00
differential: update module (#23629)
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
@@ -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})
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
)
|
||||
};
|
||||
Reference in New Issue
Block a user