forked from Archive/PX4-Autopilot
Differential Drive Guidance: Add guidance
also add dependency on control allocation parameter CA_R_REV Differential Drive Guidance: Added mission logic Differential Drive Guidance Differential Drive Guidance Differential Guidance: Inlcude library Differential Guidance: Compiles, does not work though Differential Guidance: Works somewhat Differential Guidance: Temp Differential Guidance: Tuning Differeital Drive Guidance: Remove waypoint mover Differential Guidance: Fixed accuracy issue by converting from float to double Differential Guidance: rebased on differentialdrive and improved waypoint accuracy Temp Differential Guidance: cleanup temp
This commit is contained in:
PerFrivik
Matthias Grob
parent
0f3925b21d
commit
e457a5baed
|
|
@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds)
|
|||
|
||||
float32 speed # [m/s] collective roll-off speed in body x-axis
|
||||
float32 yaw_rate # [rad/s] yaw rate
|
||||
|
||||
# TOPICS feed_forward_differential_drive_setpoint closed_loop_differential_drive_setpoint
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
############################################################################
|
||||
|
||||
add_subdirectory(DifferentialDriveKinematics)
|
||||
add_subdirectory(DifferentialDriveGuidance)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__differential_drive_control
|
||||
|
|
@ -41,7 +42,10 @@ px4_add_module(
|
|||
DifferentialDriveControl.hpp
|
||||
DEPENDS
|
||||
DifferentialDriveKinematics
|
||||
DifferentialDriveGuidance
|
||||
pid
|
||||
px4_work_queue
|
||||
modules__control_allocator # for parameter CA_R_REV
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
|
|
|||
|
|
@ -43,6 +43,9 @@ DifferentialDriveControl::DifferentialDriveControl() :
|
|||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
updateParams();
|
||||
|
||||
pid_init(&_angular_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
}
|
||||
|
||||
bool DifferentialDriveControl::init()
|
||||
|
|
@ -54,12 +57,29 @@ bool DifferentialDriveControl::init()
|
|||
void DifferentialDriveControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
pid_set_parameters(&_angular_velocity_pid,
|
||||
_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(&_speed_pid,
|
||||
_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
|
||||
|
||||
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
|
||||
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
|
||||
|
||||
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
|
||||
_differential_drive_kinematics.setMaxSpeed(_max_speed);
|
||||
_differential_guidance_controller.setMaxSpeed(_max_speed);
|
||||
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
|
||||
_differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity);
|
||||
}
|
||||
|
||||
void DifferentialDriveControl::Run()
|
||||
|
|
@ -70,23 +90,45 @@ void DifferentialDriveControl::Run()
|
|||
}
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
const double dt = static_cast<double>(math::min((now - _time_stamp_last), kTimeoutUs)) / 1e6;
|
||||
_time_stamp_last = now;
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
parameter_update_s pupdate{};
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
vehicle_control_mode_s vehicle_control_mode;
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
|
||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||
_armed = vehicle_control_mode.flag_armed;
|
||||
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
|
||||
_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
|
||||
_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
|
||||
}
|
||||
}
|
||||
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
_vehicle_attitude_sub.copy(&_vehicle_attitude);
|
||||
|
||||
_vehicle_yaw = matrix::Eulerf(matrix::Quatf(_vehicle_attitude.q)).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_angular_velocity_sub.updated()) {
|
||||
_vehicle_angular_velocity_sub.copy(&_vehicle_angular_velocity);
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
_vehicle_local_position_sub.copy(&_vehicle_local_position);
|
||||
|
||||
matrix::Vector3f ground_speed(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
|
||||
|
||||
// rotate the velocity vector from the local frame to the body frame
|
||||
_velocity_in_body_frame = Quatf(_vehicle_attitude.q).rotateVectorInverse(ground_speed);
|
||||
}
|
||||
|
||||
if (_manual_driving) {
|
||||
// Manual mode
|
||||
// directly produce setpoints from the manual control setpoint (joystick)
|
||||
|
|
@ -98,18 +140,46 @@ void DifferentialDriveControl::Run()
|
|||
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
|
||||
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
|
||||
_max_angular_velocity;
|
||||
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
|
||||
_feed_forward_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_mission_driving) {
|
||||
// Mission mode
|
||||
// directly receive setpoints from the guidance library
|
||||
matrix::Vector2f guidance_output =
|
||||
_differential_guidance_controller.computeGuidance(
|
||||
_vehicle_yaw,
|
||||
_vehicle_angular_velocity.xyz[2],
|
||||
dt
|
||||
);
|
||||
|
||||
_differential_drive_setpoint.timestamp = now;
|
||||
_differential_drive_setpoint.speed = guidance_output(0);
|
||||
_differential_drive_setpoint.yaw_rate = guidance_output(1);
|
||||
_closed_loop_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
|
||||
}
|
||||
|
||||
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
|
||||
// no need for any bools, just check if the topic is updated and update the setpoint
|
||||
if (_feed_forward_differential_drive_setpoint_sub.updated()) {
|
||||
_feed_forward_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
|
||||
|
||||
// publish data to actuator_motors (output module)
|
||||
// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
|
||||
_speed_pid_output = 0;
|
||||
_angular_velocity_pid_output = 0;
|
||||
}
|
||||
|
||||
if (_closed_loop_differential_drive_setpoint_sub.updated()) {
|
||||
_closed_loop_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
|
||||
|
||||
_speed_pid_output = pid_calculate(&_speed_pid, _differential_drive_setpoint.speed, _velocity_in_body_frame(0), 0, dt);
|
||||
_angular_velocity_pid_output = pid_calculate(&_angular_velocity_pid, _differential_drive_setpoint.yaw_rate,
|
||||
_vehicle_angular_velocity.xyz[2], 0, dt);
|
||||
}
|
||||
|
||||
// get the normalized wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
|
||||
Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
|
||||
_differential_drive_setpoint.speed,
|
||||
_differential_drive_setpoint.yaw_rate);
|
||||
_differential_drive_setpoint.speed + _speed_pid_output,
|
||||
_differential_drive_setpoint.yaw_rate + _angular_velocity_pid_output);
|
||||
|
||||
// Check if max_angular_wheel_speed is zero
|
||||
const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
|
||||
|
|
|
|||
|
|
@ -48,6 +48,13 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
|
|
@ -55,9 +62,15 @@
|
|||
|
||||
// Standard library includes
|
||||
#include <math.h>
|
||||
#include <lib/pid/pid.h>
|
||||
|
||||
// Local includes
|
||||
#include <DifferentialDriveKinematics.hpp>
|
||||
#include <DifferentialDriveGuidance.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out
|
||||
|
||||
namespace differential_drive_control
|
||||
{
|
||||
|
|
@ -86,24 +99,50 @@ protected:
|
|||
private:
|
||||
void Run() override;
|
||||
|
||||
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
|
||||
uORB::Subscription _feed_forward_differential_drive_setpoint_sub{ORB_ID(feed_forward_differential_drive_setpoint)};
|
||||
uORB::Subscription _closed_loop_differential_drive_setpoint_sub{ORB_ID(closed_loop_differential_drive_setpoint)};
|
||||
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_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
||||
uORB::Publication<differential_drive_setpoint_s> _feed_forward_differential_drive_setpoint_pub{ORB_ID(feed_forward_differential_drive_setpoint)};
|
||||
uORB::Publication<differential_drive_setpoint_s> _closed_loop_differential_drive_setpoint_pub{ORB_ID(closed_loop_differential_drive_setpoint)};
|
||||
|
||||
differential_drive_setpoint_s _differential_drive_setpoint{};
|
||||
DifferentialDriveKinematics _differential_drive_kinematics{};
|
||||
|
||||
vehicle_attitude_s _vehicle_attitude{};
|
||||
vehicle_angular_velocity_s _vehicle_angular_velocity{};
|
||||
vehicle_local_position_s _vehicle_local_position{};
|
||||
bool _armed = false;
|
||||
bool _manual_driving = false;
|
||||
bool _mission_driving = false;
|
||||
|
||||
hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
|
||||
|
||||
DifferentialDriveKinematics _differential_drive_kinematics;
|
||||
DifferentialDriveGuidance _differential_guidance_controller{this};
|
||||
|
||||
float _max_speed{0.f};
|
||||
float _max_angular_velocity{0.f};
|
||||
|
||||
float _vehicle_yaw{0.f};
|
||||
Vector3f _velocity_in_body_frame{0.f, 0.f, 0.f};
|
||||
|
||||
PID_t _angular_velocity_pid; ///< The PID controller for yaw rate.
|
||||
PID_t _speed_pid; ///< The PID controller for velocity.
|
||||
|
||||
float _speed_pid_output{0.f};
|
||||
float _angular_velocity_pid_output{0.f};
|
||||
|
||||
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,
|
||||
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
|
||||
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
|
||||
(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023 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(DifferentialDriveGuidance
|
||||
DifferentialDriveGuidance.cpp
|
||||
DifferentialDriveGuidance.hpp
|
||||
)
|
||||
|
||||
target_compile_options(DifferentialDriveGuidance PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
# px4_add_unit_gtest(SRC DifferentialDriveGuidanceTest.cpp LINKLIBS DifferentialDriveGuidance)
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#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);
|
||||
}
|
||||
|
||||
matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_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 - vehicle_yaw);
|
||||
|
||||
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);
|
||||
|
||||
if (_next_waypoint != next_waypoint) {
|
||||
if (fabsf(heading_error) < 0.1f) {
|
||||
_currentState = GuidanceState::DRIVING;
|
||||
|
||||
} else {
|
||||
_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;
|
||||
}
|
||||
|
||||
matrix::Vector2f output;
|
||||
float desired_speed = 0.f;
|
||||
|
||||
switch (_currentState) {
|
||||
case GuidanceState::TURNING:
|
||||
desired_speed = 0.f;
|
||||
break;
|
||||
|
||||
case GuidanceState::DRIVING:
|
||||
desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.4f,
|
||||
_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);
|
||||
|
||||
output(0) = math::constrain(desired_speed, -_max_speed, _max_speed);
|
||||
output(1) = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -0,0 +1,147 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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.h>
|
||||
#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 <matrix/matrix/Matrix.hpp>
|
||||
|
||||
#include <lib/motion_planning/PositionSmoothing.hpp>
|
||||
#include <lib/motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#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);
|
||||
|
||||
/**
|
||||
* @brief Default destructor.
|
||||
*/
|
||||
~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.
|
||||
* @return A 2D vector containing the computed guidance (speed in m/s, yaw rate in rad/s).
|
||||
*/
|
||||
matrix::Vector2f computeGuidance(float vehicle_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)};
|
||||
|
||||
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 _next_waypoint; ///< The next 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
|
||||
)
|
||||
|
||||
};
|
||||
|
|
@ -52,4 +52,72 @@ parameters:
|
|||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 10
|
||||
default: 0.3
|
||||
RDD_P_HEADING:
|
||||
description:
|
||||
short: Proportional gain for heading controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1.0
|
||||
RDD_P_SPEED:
|
||||
description:
|
||||
short: Proportional gain for speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1.0
|
||||
RDD_I_SPEED:
|
||||
description:
|
||||
short: Integral gain for ground speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.0
|
||||
RDD_P_ANG_VEL:
|
||||
description:
|
||||
short: Proportional gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1.0
|
||||
RDD_I_ANG_VEL:
|
||||
description:
|
||||
short: Integral gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.0
|
||||
RDD_MAX_JERK:
|
||||
description:
|
||||
short: Maximum jerk
|
||||
long: Limit for forwards acc/deceleration change.
|
||||
type: float
|
||||
unit: m/s^3
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.5
|
||||
RDD_MAX_ACCEL:
|
||||
description:
|
||||
short: Maximum acceleration
|
||||
long: Maximum acceleration is used to limit the acceleration of the rover
|
||||
type: float
|
||||
unit: m/s^2
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.5
|
||||
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("commander_state");
|
||||
add_topic("config_overrides");
|
||||
add_topic("cpuload");
|
||||
add_optional_topic("differential_drive_setpoint", 100);
|
||||
add_optional_topic("feed_forward_differential_drive_setpoint", 100);
|
||||
add_optional_topic("closed_loop_differential_drive_setpoint", 100);
|
||||
add_optional_topic("external_ins_attitude");
|
||||
add_optional_topic("external_ins_global_position");
|
||||
add_optional_topic("external_ins_local_position");
|
||||
|
|
|
|||
|
|
@ -127,7 +127,10 @@ subscriptions:
|
|||
- topic: /fmu/in/vehicle_rates_setpoint
|
||||
type: px4_msgs::msg::VehicleRatesSetpoint
|
||||
|
||||
- topic: /fmu/in/differential_drive_setpoint
|
||||
- topic: /fmu/in/feed_forward_differential_drive_setpoint
|
||||
type: px4_msgs::msg::DifferentialDriveSetpoint
|
||||
|
||||
- topic: /fmu/in/closed_loop_differential_drive_setpoint
|
||||
type: px4_msgs::msg::DifferentialDriveSetpoint
|
||||
|
||||
- topic: /fmu/in/vehicle_visual_odometry
|
||||
|
|
|
|||
Loading…
Reference in New Issue