mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 02:43:34 +08:00
differential: restructure and update module (#23430)
* differential: rename module * differential: restructure and update module
This commit is contained in:
chfriedrich98
@@ -0,0 +1,49 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(RoverDifferentialGuidance)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__rover_differential
|
||||
MAIN rover_differential
|
||||
SRCS
|
||||
RoverDifferential.cpp
|
||||
RoverDifferential.hpp
|
||||
DEPENDS
|
||||
RoverDifferentialGuidance
|
||||
px4_work_queue
|
||||
modules__control_allocator # for parameter CA_R_REV
|
||||
pure_pursuit
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
@@ -0,0 +1,6 @@
|
||||
menuconfig MODULES_ROVER_DIFFERENTIAL
|
||||
bool "rover_differential"
|
||||
default n
|
||||
depends on MODULES_CONTROL_ALLOCATOR
|
||||
---help---
|
||||
Enable support for control of differential rovers
|
||||
@@ -0,0 +1,256 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "RoverDifferential.hpp"
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
RoverDifferential::RoverDifferential() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
updateParams();
|
||||
_rover_differential_status_pub.advertise();
|
||||
pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
}
|
||||
|
||||
bool RoverDifferential::init()
|
||||
{
|
||||
ScheduleOnInterval(10_ms); // 100 Hz
|
||||
return true;
|
||||
}
|
||||
|
||||
void RoverDifferential::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
|
||||
|
||||
pid_set_parameters(&_pid_yaw_rate,
|
||||
_param_rd_p_gain_yaw_rate.get(), // Proportional gain
|
||||
_param_rd_i_gain_yaw_rate.get(), // Integral gain
|
||||
0.f, // Derivative gain
|
||||
1.f, // Integral limit
|
||||
1.f); // Output limit
|
||||
|
||||
}
|
||||
|
||||
void RoverDifferential::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
// uORB subscriber updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s parameter_update;
|
||||
_parameter_update_sub.copy(¶meter_update);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
_nav_state = vehicle_status.nav_state;
|
||||
}
|
||||
|
||||
if (_vehicle_angular_velocity_sub.updated()) {
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity{};
|
||||
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
|
||||
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
|
||||
}
|
||||
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
||||
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
_vehicle_forward_speed = velocity_in_body_frame(0);
|
||||
}
|
||||
|
||||
// Navigation modes
|
||||
switch (_nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
_differential_setpoint.throttle = manual_control_setpoint.throttle;
|
||||
_differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
|
||||
|
||||
}
|
||||
|
||||
_differential_setpoint.closed_loop_yaw_rate = false;
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
_differential_setpoint.throttle = manual_control_setpoint.throttle;
|
||||
_differential_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.roll,
|
||||
-1.f, 1.f,
|
||||
-_max_yaw_rate, _max_yaw_rate);
|
||||
}
|
||||
|
||||
_differential_setpoint.closed_loop_yaw_rate = true;
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
_differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed,
|
||||
_nav_state);
|
||||
break;
|
||||
|
||||
default: // Unimplemented nav states will stop the rover
|
||||
_differential_setpoint.throttle = 0.f;
|
||||
_differential_setpoint.yaw_rate = 0.f;
|
||||
_differential_setpoint.closed_loop_yaw_rate = false;
|
||||
break;
|
||||
}
|
||||
|
||||
float speed_diff_normalized = _differential_setpoint.yaw_rate;
|
||||
|
||||
// Closed loop yaw rate control
|
||||
if (_differential_setpoint.closed_loop_yaw_rate) {
|
||||
if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) {
|
||||
speed_diff_normalized = 0.f;
|
||||
pid_reset_integral(&_pid_yaw_rate);
|
||||
|
||||
} else {
|
||||
const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward
|
||||
speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_speed.get(),
|
||||
_param_rd_max_speed.get(), -1.f, 1.f);
|
||||
speed_diff_normalized = math::constrain(speed_diff_normalized +
|
||||
pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
|
||||
-1.f, 1.f); // Feedback
|
||||
}
|
||||
|
||||
} else {
|
||||
pid_reset_integral(&_pid_yaw_rate);
|
||||
}
|
||||
|
||||
// Publish rover differential status (logging)
|
||||
rover_differential_status_s rover_differential_status{};
|
||||
rover_differential_status.timestamp = _timestamp;
|
||||
rover_differential_status.actual_speed = _vehicle_forward_speed;
|
||||
rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate;
|
||||
rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate;
|
||||
rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
|
||||
_rover_differential_status_pub.publish(rover_differential_status);
|
||||
|
||||
// Publish to motors
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.reversible_flags = _param_r_rev.get();
|
||||
computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control);
|
||||
actuator_motors.timestamp = _timestamp;
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
}
|
||||
|
||||
matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff)
|
||||
{
|
||||
float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff);
|
||||
|
||||
if (combined_velocity > 1.0f) { // Prioritize yaw rate
|
||||
float excess_velocity = fabsf(combined_velocity - 1.0f);
|
||||
forward_speed -= sign(forward_speed) * excess_velocity;
|
||||
}
|
||||
|
||||
// Calculate the left and right wheel speeds
|
||||
return Vector2f(forward_speed - speed_diff,
|
||||
forward_speed + speed_diff);
|
||||
}
|
||||
|
||||
int RoverDifferential::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
RoverDifferential *instance = new RoverDifferential();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int RoverDifferential::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unk_timestampn command");
|
||||
}
|
||||
|
||||
int RoverDifferential::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_ERR("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Rover Differential controller.
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("rover_differential", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int rover_differential_main(int argc, char *argv[])
|
||||
{
|
||||
return RoverDifferential::main(argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
// uORB includes
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/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>
|
||||
|
||||
// Standard libraries
|
||||
#include <lib/pid/pid.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
// Local includes
|
||||
#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class RoverDifferential : public ModuleBase<RoverDifferential>, public ModuleParams,
|
||||
public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
RoverDifferential();
|
||||
~RoverDifferential() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
/**
|
||||
* @brief Computes motor commands for differential drive.
|
||||
*
|
||||
* @param forward_speed Linear velocity along the x-axis.
|
||||
* @param speed_diff Speed difference between left and right wheels.
|
||||
* @return matrix::Vector2f Motor velocities for the right and left motors.
|
||||
*/
|
||||
matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff);
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
// uORB Subscriptions
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
// uORB 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)};
|
||||
|
||||
// Instances
|
||||
RoverDifferentialGuidance _rover_differential_guidance{this};
|
||||
|
||||
// Variables
|
||||
float _vehicle_body_yaw_rate{0.f};
|
||||
float _vehicle_forward_speed{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
float _max_yaw_rate{0.f};
|
||||
int _nav_state{0};
|
||||
matrix::Quatf _vehicle_attitude_quaternion{};
|
||||
hrt_abstime _timestamp{0};
|
||||
PID_t _pid_yaw_rate; // The PID controller for yaw rate
|
||||
RoverDifferentialGuidance::differential_setpoint _differential_setpoint;
|
||||
|
||||
// Constants
|
||||
static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::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
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(RoverDifferentialGuidance
|
||||
RoverDifferentialGuidance.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(RoverDifferentialGuidance PUBLIC pid)
|
||||
target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
+247
@@ -0,0 +1,247 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "RoverDifferentialGuidance.hpp"
|
||||
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
_rover_differential_guidance_status_pub.advertise();
|
||||
pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
|
||||
}
|
||||
|
||||
void RoverDifferentialGuidance::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
|
||||
pid_set_parameters(&_pid_heading,
|
||||
_param_rd_p_gain_heading.get(), // Proportional gain
|
||||
_param_rd_i_gain_heading.get(), // Integral gain
|
||||
0.f, // Derivative gain
|
||||
_max_yaw_rate, // Integral limit
|
||||
_max_yaw_rate); // Output limit
|
||||
pid_set_parameters(&_pid_throttle,
|
||||
_param_rd_p_gain_speed.get(), // Proportional gain
|
||||
_param_rd_i_gain_speed.get(), // Integral gain
|
||||
0.f, // Derivative gain
|
||||
1.f, // Integral limit
|
||||
1.f); // Output limit
|
||||
}
|
||||
|
||||
RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw,
|
||||
const float actual_speed, const int nav_state)
|
||||
{
|
||||
// Initializations
|
||||
bool mission_finished{false};
|
||||
float desired_speed{0.f};
|
||||
float desired_yaw_rate{0.f};
|
||||
hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
// uORB subscriber updates
|
||||
if (_vehicle_global_position_sub.updated()) {
|
||||
vehicle_global_position_s vehicle_global_position{};
|
||||
_vehicle_global_position_sub.copy(&vehicle_global_position);
|
||||
_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
|
||||
}
|
||||
|
||||
if (_local_position_sub.updated()) {
|
||||
vehicle_local_position_s local_position{};
|
||||
_local_position_sub.copy(&local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(local_position.x, local_position.y);
|
||||
}
|
||||
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
updateWaypoints();
|
||||
}
|
||||
|
||||
if (_mission_result_sub.updated()) {
|
||||
mission_result_s mission_result{};
|
||||
_mission_result_sub.copy(&mission_result);
|
||||
mission_finished = mission_result.finished;
|
||||
}
|
||||
|
||||
if (_home_position_sub.updated()) {
|
||||
home_position_s home_position{};
|
||||
_home_position_sub.copy(&home_position);
|
||||
_home_position = Vector2d(home_position.lat, home_position.lon);
|
||||
}
|
||||
|
||||
const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
|
||||
math::max(actual_speed, 0.f));
|
||||
|
||||
const float heading_error = matrix::wrap_pi(desired_heading - yaw);
|
||||
|
||||
const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_curr_wp(0),
|
||||
_curr_wp(1));
|
||||
|
||||
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
|
||||
&& distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch
|
||||
mission_finished = true;
|
||||
}
|
||||
|
||||
// State machine
|
||||
if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) {
|
||||
if (_currentState == GuidanceState::STOPPED) {
|
||||
_currentState = GuidanceState::DRIVING;
|
||||
}
|
||||
|
||||
if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
|
||||
pid_reset_integral(&_pid_heading);
|
||||
_currentState = GuidanceState::SPOT_TURNING;
|
||||
|
||||
} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
|
||||
pid_reset_integral(&_pid_heading);
|
||||
_currentState = GuidanceState::DRIVING;
|
||||
}
|
||||
|
||||
} else { // Mission finished or delay command
|
||||
_currentState = GuidanceState::STOPPED;
|
||||
}
|
||||
|
||||
// Guidance logic
|
||||
switch (_currentState) {
|
||||
case GuidanceState::DRIVING: {
|
||||
desired_speed = _param_rd_miss_spd_def.get();
|
||||
|
||||
if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
|
||||
desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
|
||||
_param_rd_max_accel.get(), distance_to_next_wp, 0.0f);
|
||||
desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
|
||||
}
|
||||
|
||||
desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt);
|
||||
} break;
|
||||
|
||||
case GuidanceState::SPOT_TURNING:
|
||||
if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop
|
||||
desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case GuidanceState::STOPPED:
|
||||
default:
|
||||
desired_speed = 0.f;
|
||||
desired_yaw_rate = 0.f;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
// Closed loop speed control
|
||||
float throttle{0.f};
|
||||
|
||||
if (fabsf(desired_speed) < FLT_EPSILON) {
|
||||
pid_reset_integral(&_pid_throttle);
|
||||
|
||||
} else {
|
||||
throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0,
|
||||
dt);
|
||||
|
||||
if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
|
||||
throttle += math::interpolate<float>(desired_speed,
|
||||
0.f, _param_rd_max_speed.get(),
|
||||
0.f, 1.f);
|
||||
}
|
||||
}
|
||||
|
||||
// Publish differential controller status (logging)
|
||||
_rover_differential_guidance_status.timestamp = _timestamp;
|
||||
_rover_differential_guidance_status.desired_speed = desired_speed;
|
||||
_rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral;
|
||||
_rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
|
||||
_rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral;
|
||||
_rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error;
|
||||
_rover_differential_guidance_status.state_machine = (uint8_t) _currentState;
|
||||
_rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status);
|
||||
|
||||
// Return setpoints
|
||||
differential_setpoint differential_setpoint_temp;
|
||||
differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f);
|
||||
differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate,
|
||||
_max_yaw_rate);
|
||||
differential_setpoint_temp.closed_loop_yaw_rate = true;
|
||||
return differential_setpoint_temp;
|
||||
}
|
||||
|
||||
void RoverDifferentialGuidance::updateWaypoints()
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet{};
|
||||
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
|
||||
|
||||
// Global waypoint coordinates
|
||||
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
|
||||
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
|
||||
_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
|
||||
|
||||
} else {
|
||||
_curr_wp = Vector2d(0, 0);
|
||||
}
|
||||
|
||||
if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
|
||||
&& PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
|
||||
_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
|
||||
|
||||
} else {
|
||||
_prev_wp = _curr_pos;
|
||||
}
|
||||
|
||||
if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
|
||||
&& PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
|
||||
_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
|
||||
|
||||
} else {
|
||||
_next_wp = _home_position;
|
||||
}
|
||||
|
||||
// NED waypoint coordinates
|
||||
_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
|
||||
_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
|
||||
|
||||
}
|
||||
+163
@@ -0,0 +1,163 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/pure_pursuit/PurePursuit.hpp>
|
||||
|
||||
// uORB includes
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/rover_differential_guidance_status.h>
|
||||
|
||||
// Standard libraries
|
||||
#include <lib/pid/pid.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <math.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
/**
|
||||
* @brief Enum class for the different states of guidance.
|
||||
*/
|
||||
enum class GuidanceState {
|
||||
SPOT_TURNING, // The vehicle is currently turning on the spot.
|
||||
DRIVING, // The vehicle is currently driving.
|
||||
STOPPED // The vehicle is stopped.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Class for differential rover guidance.
|
||||
*/
|
||||
class RoverDifferentialGuidance : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for RoverDifferentialGuidance.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
RoverDifferentialGuidance(ModuleParams *parent);
|
||||
~RoverDifferentialGuidance() = default;
|
||||
|
||||
struct differential_setpoint {
|
||||
float throttle{0.f};
|
||||
float yaw_rate{0.f};
|
||||
bool closed_loop_yaw_rate{false};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Compute guidance for the vehicle.
|
||||
* @param yaw The yaw orientation of the vehicle in radians.
|
||||
* @param actual_speed The velocity of the vehicle in m/s.
|
||||
* @param dt The time step in seconds.
|
||||
* @param nav_state Navigation state of the rover.
|
||||
*/
|
||||
RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed,
|
||||
int nav_state);
|
||||
|
||||
/**
|
||||
* @brief Update global/ned waypoint coordinates
|
||||
*/
|
||||
void updateWaypoints();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_differential_guidance_status_s> _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)};
|
||||
rover_differential_guidance_status_s _rover_differential_guidance_status{};
|
||||
|
||||
// Variables
|
||||
MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
|
||||
GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance.
|
||||
PurePursuit _pure_pursuit{this}; // Pure pursuit library
|
||||
hrt_abstime _timestamp{0};
|
||||
float _max_yaw_rate{0.f};
|
||||
|
||||
|
||||
// Waypoints
|
||||
Vector2d _curr_pos{};
|
||||
Vector2f _curr_pos_ned{};
|
||||
Vector2d _prev_wp{};
|
||||
Vector2f _prev_wp_ned{};
|
||||
Vector2d _curr_wp{};
|
||||
Vector2f _curr_wp_ned{};
|
||||
Vector2d _next_wp{};
|
||||
Vector2d _home_position{};
|
||||
|
||||
// Controllers
|
||||
PID_t _pid_heading; // The PID controller for the heading
|
||||
PID_t _pid_throttle; // The PID controller for velocity
|
||||
|
||||
// Constants
|
||||
static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s]
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RD_HEADING_P>) _param_rd_p_gain_heading,
|
||||
(ParamFloat<px4::params::RD_HEADING_I>) _param_rd_i_gain_heading,
|
||||
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
|
||||
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
|
||||
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
|
||||
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
|
||||
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
|
||||
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
|
||||
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
|
||||
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
|
||||
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
|
||||
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,172 @@
|
||||
module_name: Rover Differential
|
||||
|
||||
parameters:
|
||||
- group: Rover Differential
|
||||
definitions:
|
||||
|
||||
RD_WHEEL_TRACK:
|
||||
description:
|
||||
short: Wheel track
|
||||
long: Distance from the center of the right wheel to the center of the left wheel
|
||||
type: float
|
||||
unit: m
|
||||
min: 0.001
|
||||
max: 100
|
||||
increment: 0.001
|
||||
decimal: 3
|
||||
default: 0.5
|
||||
|
||||
RD_MAN_YAW_SCALE:
|
||||
description:
|
||||
short: Manual yaw rate scale
|
||||
long: |
|
||||
In manual mode the setpoint for the yaw rate received from the rc remote
|
||||
is scaled by this value.
|
||||
type: float
|
||||
min: 0.01
|
||||
max: 1
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RD_HEADING_P:
|
||||
description:
|
||||
short: Proportional gain for heading controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RD_HEADING_I:
|
||||
description:
|
||||
short: Integral gain for heading controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0.1
|
||||
|
||||
RD_SPEED_P:
|
||||
description:
|
||||
short: Proportional gain for speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RD_SPEED_I:
|
||||
description:
|
||||
short: Integral gain for ground speed controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0
|
||||
|
||||
RD_YAW_RATE_P:
|
||||
description:
|
||||
short: Proportional gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RD_YAW_RATE_I:
|
||||
description:
|
||||
short: Integral gain for angular velocity controller
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 0
|
||||
|
||||
RD_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
|
||||
|
||||
RD_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
|
||||
|
||||
RD_MAX_SPEED:
|
||||
description:
|
||||
short: Maximum speed the rover can drive
|
||||
long: This parameter is used to map desired speeds to normalized motor commands.
|
||||
type: float
|
||||
unit: m/s
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 7
|
||||
|
||||
RD_MAX_YAW_RATE:
|
||||
description:
|
||||
short: Maximum allowed yaw rate for the rover.
|
||||
long: |
|
||||
This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
|
||||
type: float
|
||||
unit: deg/s
|
||||
min: 0.01
|
||||
max: 1000
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 90
|
||||
|
||||
RD_MISS_SPD_DEF:
|
||||
description:
|
||||
short: Default rover speed during a mission
|
||||
type: float
|
||||
unit: m/s
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RD_TRANS_TRN_DRV:
|
||||
description:
|
||||
short: Heading error threshhold to switch from spot turning to driving
|
||||
type: float
|
||||
unit: rad
|
||||
min: 0.001
|
||||
max: 180
|
||||
increment: 0.01
|
||||
decimal: 3
|
||||
default: 0.0872665
|
||||
|
||||
RD_TRANS_DRV_TRN:
|
||||
description:
|
||||
short: Heading error threshhold to switch from driving to spot turning
|
||||
type: float
|
||||
unit: rad
|
||||
min: 0.001
|
||||
max: 180
|
||||
increment: 0.01
|
||||
decimal: 3
|
||||
default: 0.174533
|
||||
Reference in New Issue
Block a user