mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 13:17:35 +08:00
ackermann: restructure module
This commit is contained in:
chfriedrich98
committed by
chfriedrich98
chfriedrich98
parent
79ec39e561
commit
6a7edac10d
@@ -16,15 +16,17 @@ param set-default NAV_ACC_RAD 0.5
|
||||
param set-default RA_ACC_RAD_GAIN 2
|
||||
param set-default RA_ACC_RAD_MAX 3
|
||||
param set-default RA_MAX_ACCEL 1.5
|
||||
param set-default RA_MAX_DECEL 10
|
||||
param set-default RA_MAX_JERK 15
|
||||
param set-default RA_MAX_SPEED 3
|
||||
param set-default RA_MAX_SPEED 5
|
||||
param set-default RA_MAX_THR_SPEED 6
|
||||
param set-default RA_MAX_STR_ANG 0.5236
|
||||
param set-default RA_MAX_STR_RATE 360
|
||||
param set-default RA_MISS_VEL_DEF 3
|
||||
param set-default RA_MISS_VEL_GAIN 5
|
||||
param set-default RA_MISS_VEL_MIN 1
|
||||
param set-default RA_MISS_SPD_DEF 5
|
||||
param set-default RA_MISS_SPD_GAIN 5
|
||||
param set-default RA_MISS_SPD_MIN 1
|
||||
param set-default RA_SPEED_I 0.01
|
||||
param set-default RA_SPEED_P 2
|
||||
param set-default RA_SPEED_P 0.1
|
||||
param set-default RA_WHEEL_BASE 0.321
|
||||
|
||||
# Pure Pursuit parameters
|
||||
|
||||
@@ -185,6 +185,7 @@ set(msg_files
|
||||
RegisterExtComponentReply.msg
|
||||
RegisterExtComponentRequest.msg
|
||||
RoverAckermannGuidanceStatus.msg
|
||||
RoverAckermannSetpoint.msg
|
||||
RoverAckermannStatus.msg
|
||||
RoverDifferentialGuidanceStatus.msg
|
||||
RoverDifferentialSetpoint.msg
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 desired_speed # [m/s] Rover desired ground speed
|
||||
float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
|
||||
float32 heading_error # [deg] Heading error of the pure pursuit controller
|
||||
float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
|
||||
|
||||
# TOPICS rover_ackermann_guidance_status
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
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] Desired normalized forward speed for the rover
|
||||
float32 steering_setpoint # [rad/s] Desired steering for the rover
|
||||
float32 steering_setpoint_normalized # [-1, 1] Desired normalized steering for the rover
|
||||
|
||||
# TOPICS rover_ackermann_setpoint
|
||||
@@ -1,7 +1,9 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 throttle_setpoint # [-1, 1] Normalized throttle setpoint
|
||||
float32 steering_setpoint # [-1, 1] Normalized steering setpoint
|
||||
float32 actual_speed # [m/s] Rover ground speed
|
||||
float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
|
||||
float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
|
||||
float32 steering_setpoint_normalized # [-1, 1] Normalized steering setpoint
|
||||
float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate
|
||||
float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller
|
||||
|
||||
# TOPICS rover_ackermann_status
|
||||
|
||||
@@ -104,6 +104,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("px4io_status");
|
||||
add_topic("radio_status");
|
||||
add_optional_topic("rover_ackermann_guidance_status", 100);
|
||||
add_optional_topic("rover_ackermann_setpoint", 100);
|
||||
add_optional_topic("rover_ackermann_status", 100);
|
||||
add_optional_topic("rover_differential_guidance_status", 100);
|
||||
add_optional_topic("rover_differential_setpoint", 100);
|
||||
|
||||
@@ -32,6 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(RoverAckermannGuidance)
|
||||
add_subdirectory(RoverAckermannControl)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__rover_ackermann
|
||||
@@ -41,6 +42,7 @@ px4_add_module(
|
||||
RoverAckermann.hpp
|
||||
DEPENDS
|
||||
RoverAckermannGuidance
|
||||
RoverAckermannControl
|
||||
px4_work_queue
|
||||
SlewRate
|
||||
pure_pursuit
|
||||
|
||||
@@ -33,14 +33,11 @@
|
||||
|
||||
#include "RoverAckermann.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace matrix;
|
||||
|
||||
RoverAckermann::RoverAckermann() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
_rover_ackermann_status_pub.advertise();
|
||||
_rover_ackermann_setpoint_pub.advertise();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
@@ -53,16 +50,6 @@ bool RoverAckermann::init()
|
||||
void RoverAckermann::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
// Update slew rates
|
||||
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
|
||||
_throttle_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get());
|
||||
}
|
||||
|
||||
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
|
||||
_steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) /
|
||||
_param_ra_max_steer_angle.get());
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::Run()
|
||||
@@ -75,45 +62,47 @@ void RoverAckermann::Run()
|
||||
|
||||
updateSubscriptions();
|
||||
|
||||
// Timestamps
|
||||
hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
// Generate and publish speed and steering setpoints
|
||||
hrt_abstime timestamp = hrt_absolute_time();
|
||||
|
||||
// Generate motor setpoints
|
||||
if (_armed) {
|
||||
switch (_nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
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)) {
|
||||
_motor_setpoint.steering = manual_control_setpoint.roll;
|
||||
_motor_setpoint.throttle = manual_control_setpoint.throttle;
|
||||
}
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
rover_ackermann_setpoint_s rover_ackermann_setpoint{};
|
||||
rover_ackermann_setpoint.timestamp = timestamp;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
|
||||
rover_ackermann_setpoint.steering_setpoint = NAN;
|
||||
rover_ackermann_setpoint.steering_setpoint_normalized = manual_control_setpoint.roll;
|
||||
_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
|
||||
}
|
||||
|
||||
} break;
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
_motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
|
||||
break;
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
_ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed);
|
||||
break;
|
||||
|
||||
default: // Unimplemented nav states will stop the rover
|
||||
_motor_setpoint.steering = 0.f;
|
||||
_motor_setpoint.throttle = 0.f;
|
||||
_throttle_with_accel_limit.setForcedValue(0.f);
|
||||
_steering_with_rate_limit.setForcedValue(0.f);
|
||||
break;
|
||||
}
|
||||
|
||||
} else { // Reset on disarm
|
||||
_motor_setpoint.steering = 0.f;
|
||||
_motor_setpoint.throttle = 0.f;
|
||||
_throttle_with_accel_limit.setForcedValue(0.f);
|
||||
_steering_with_rate_limit.setForcedValue(0.f);
|
||||
default: // Unimplemented nav states will stop the rover
|
||||
rover_ackermann_setpoint_s rover_ackermann_setpoint{};
|
||||
rover_ackermann_setpoint.timestamp = timestamp;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint_normalized = 0.f;
|
||||
rover_ackermann_setpoint.steering_setpoint = NAN;
|
||||
rover_ackermann_setpoint.steering_setpoint_normalized = 0.f;
|
||||
_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
|
||||
break;
|
||||
}
|
||||
|
||||
publishMotorSetpoints(applySlewRates(_motor_setpoint, dt));
|
||||
if (!_armed) {
|
||||
_ackermann_control.resetControllers();
|
||||
}
|
||||
|
||||
_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermann::updateSubscriptions()
|
||||
@@ -129,69 +118,20 @@ void RoverAckermann::updateSubscriptions()
|
||||
_armed = vehicle_status.arming_state == 2;
|
||||
}
|
||||
|
||||
if (_local_position_sub.updated()) {
|
||||
vehicle_local_position_s local_position{};
|
||||
_local_position_sub.copy(&local_position);
|
||||
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
|
||||
_actual_speed = rover_velocity.norm();
|
||||
}
|
||||
}
|
||||
motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt)
|
||||
{
|
||||
// Sanitize actuator commands
|
||||
if (!PX4_ISFINITE(motor_setpoint.steering)) {
|
||||
motor_setpoint.steering = 0.f;
|
||||
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 (!PX4_ISFINITE(motor_setpoint.throttle)) {
|
||||
motor_setpoint.throttle = 0.f;
|
||||
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);
|
||||
}
|
||||
|
||||
// Acceleration slew rate
|
||||
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
|
||||
&& fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
|
||||
_throttle_with_accel_limit.update(motor_setpoint.throttle, dt);
|
||||
|
||||
} else {
|
||||
_throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle);
|
||||
}
|
||||
|
||||
// Steering slew rate
|
||||
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
|
||||
_steering_with_rate_limit.update(motor_setpoint.steering, dt);
|
||||
|
||||
} else {
|
||||
_steering_with_rate_limit.setForcedValue(motor_setpoint.steering);
|
||||
}
|
||||
|
||||
motor_setpoint_struct motor_setpoint_temp{};
|
||||
motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
|
||||
motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
|
||||
return motor_setpoint_temp;
|
||||
}
|
||||
|
||||
void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates)
|
||||
{
|
||||
// Publish rover Ackermann status (logging)
|
||||
rover_ackermann_status_s rover_ackermann_status{};
|
||||
rover_ackermann_status.timestamp = _timestamp;
|
||||
rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
|
||||
rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
|
||||
rover_ackermann_status.actual_speed = _actual_speed;
|
||||
_rover_ackermann_status_pub.publish(rover_ackermann_status);
|
||||
|
||||
// Publish to motor
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.reversible_flags = _param_r_rev.get();
|
||||
actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle;
|
||||
actuator_motors.timestamp = _timestamp;
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
// Publish to servo
|
||||
actuator_servos_s actuator_servos{};
|
||||
actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering;
|
||||
actuator_servos.timestamp = _timestamp;
|
||||
_actuator_servos_pub.publish(actuator_servos);
|
||||
}
|
||||
|
||||
int RoverAckermann::task_spawn(int argc, char *argv[])
|
||||
|
||||
@@ -39,27 +39,25 @@
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/slew_rate/SlewRate.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/actuator_servos.h>
|
||||
#include <uORB/topics/rover_ackermann_status.h>
|
||||
#include <uORB/topics/rover_ackermann_setpoint.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
||||
|
||||
// Standard library includes
|
||||
#include <math.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
// Local includes
|
||||
#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
|
||||
using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint;
|
||||
#include "RoverAckermannControl/RoverAckermannControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@@ -96,49 +94,25 @@ private:
|
||||
*/
|
||||
void updateSubscriptions();
|
||||
|
||||
/**
|
||||
* @brief Apply slew rates to motor setpoints.
|
||||
* @param motor_setpoint Normalized steering and throttle setpoints.
|
||||
* @param dt Time since last update [s].
|
||||
* @return Motor setpoint with applied slew rates.
|
||||
*/
|
||||
motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt);
|
||||
|
||||
/**
|
||||
* @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus.
|
||||
* @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates.
|
||||
*/
|
||||
void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates);
|
||||
|
||||
// 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_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
|
||||
// uORB publications
|
||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
|
||||
uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};
|
||||
uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
|
||||
|
||||
// Class instances
|
||||
RoverAckermannGuidance _ackermann_guidance{this};
|
||||
RoverAckermannControl _ackermann_control{this};
|
||||
|
||||
// Variables
|
||||
matrix::Quatf _vehicle_attitude_quaternion{};
|
||||
int _nav_state{0};
|
||||
motor_setpoint_struct _motor_setpoint;
|
||||
hrt_abstime _timestamp{0};
|
||||
float _actual_speed{0.f};
|
||||
SlewRate<float> _steering_with_rate_limit{0.f};
|
||||
SlewRate<float> _throttle_with_accel_limit{0.f};
|
||||
float _vehicle_forward_speed{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
bool _armed{false};
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
|
||||
(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate
|
||||
)
|
||||
};
|
||||
|
||||
@@ -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(RoverAckermannControl
|
||||
RoverAckermannControl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(RoverAckermannControl PUBLIC pid)
|
||||
target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,191 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "RoverAckermannControl.hpp"
|
||||
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
_rover_ackermann_status_pub.advertise();
|
||||
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermannControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
pid_set_parameters(&_pid_throttle,
|
||||
_param_ra_p_speed.get(), // Proportional gain
|
||||
_param_ra_i_speed.get(), // Integral gain
|
||||
0, // Derivative gain
|
||||
1, // Integral limit
|
||||
1); // Output limit
|
||||
|
||||
// Update slew rates
|
||||
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
|
||||
_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get());
|
||||
}
|
||||
|
||||
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
|
||||
_steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) /
|
||||
_param_ra_max_steer_angle.get());
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw)
|
||||
{
|
||||
// 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 ackermann setpoint
|
||||
_rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint);
|
||||
|
||||
// Speed control
|
||||
float forward_speed_normalized{0.f};
|
||||
|
||||
if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) {
|
||||
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint,
|
||||
vehicle_forward_speed, dt, false);
|
||||
|
||||
} else if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
|
||||
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint_normalized,
|
||||
vehicle_forward_speed, dt, true);
|
||||
|
||||
}
|
||||
|
||||
// Steering control
|
||||
float steering_normalized{0.f};
|
||||
|
||||
if (PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint)) {
|
||||
steering_normalized = math::interpolate<float>(_rover_ackermann_setpoint.steering_setpoint,
|
||||
-_param_ra_max_steer_angle.get(),
|
||||
_param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint
|
||||
|
||||
} else { // Use normalized setpoint
|
||||
steering_normalized = PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint_normalized) ? math::constrain(
|
||||
_rover_ackermann_setpoint.steering_setpoint_normalized, -1.f, 1.f) : 0.f;
|
||||
}
|
||||
|
||||
if (_param_ra_max_steering_rate.get() > FLT_EPSILON
|
||||
&& _param_ra_max_steer_angle.get() > FLT_EPSILON) { // Apply slew rate
|
||||
_steering_with_rate_limit.update(steering_normalized, dt);
|
||||
|
||||
} else {
|
||||
_steering_with_rate_limit.setForcedValue(steering_normalized);
|
||||
}
|
||||
|
||||
// Publish rover Ackermann status (logging)
|
||||
_rover_ackermann_status.timestamp = _timestamp;
|
||||
_rover_ackermann_status.measured_forward_speed = vehicle_forward_speed;
|
||||
_rover_ackermann_status.steering_setpoint_normalized = steering_normalized;
|
||||
_rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState();
|
||||
_rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral;
|
||||
_rover_ackermann_status_pub.publish(_rover_ackermann_status);
|
||||
|
||||
// Publish to motor
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.reversible_flags = _param_r_rev.get();
|
||||
actuator_motors.control[0] = forward_speed_normalized;
|
||||
actuator_motors.timestamp = _timestamp;
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
// Publish to servo
|
||||
actuator_servos_s actuator_servos{};
|
||||
actuator_servos.control[0] = _steering_with_rate_limit.getState();
|
||||
actuator_servos.timestamp = _timestamp;
|
||||
_actuator_servos_pub.publish(actuator_servos);
|
||||
|
||||
}
|
||||
|
||||
float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
|
||||
const float vehicle_forward_speed, const float dt, const bool normalized)
|
||||
{
|
||||
float slew_rate_normalization{1.f};
|
||||
|
||||
if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
|
||||
slew_rate_normalization = _param_ra_max_thr_speed.get() > FLT_EPSILON ? _param_ra_max_thr_speed.get() : 0.f;
|
||||
}
|
||||
|
||||
// Apply acceleration and deceleration limit
|
||||
if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) {
|
||||
if (_param_ra_max_accel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
|
||||
_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / slew_rate_normalization);
|
||||
_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
|
||||
|
||||
} else {
|
||||
_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
|
||||
|
||||
}
|
||||
|
||||
} else if (_param_ra_max_decel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
|
||||
_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_decel.get() / slew_rate_normalization);
|
||||
_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
|
||||
|
||||
} else {
|
||||
_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
|
||||
}
|
||||
|
||||
// Calculate normalized forward speed setpoint
|
||||
float forward_speed_normalized{0.f};
|
||||
|
||||
if (normalized) {
|
||||
forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState();
|
||||
|
||||
} else { // Closed loop speed control
|
||||
_rover_ackermann_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
|
||||
|
||||
if (_param_ra_max_thr_speed.get() > FLT_EPSILON) { // Feedforward
|
||||
forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
|
||||
-_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get(),
|
||||
-1.f, 1.f);
|
||||
}
|
||||
|
||||
forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
|
||||
vehicle_forward_speed, 0, dt); // Feedback
|
||||
}
|
||||
|
||||
return math::constrain(forward_speed_normalized, -1.f, 1.f);
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermannControl::resetControllers()
|
||||
{
|
||||
pid_reset_integral(&_pid_throttle);
|
||||
}
|
||||
@@ -0,0 +1,130 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_ackermann_setpoint.h>
|
||||
#include <uORB/topics/rover_ackermann_status.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/actuator_servos.h>
|
||||
|
||||
// Standard libraries
|
||||
#include <lib/pid/pid.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <math.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* @brief Class for ackermann rover guidance.
|
||||
*/
|
||||
class RoverAckermannControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for RoverAckermannControl.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
RoverAckermannControl(ModuleParams *parent);
|
||||
~RoverAckermannControl() = default;
|
||||
|
||||
/**
|
||||
* @brief Compute motor commands based on setpoints
|
||||
*/
|
||||
void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw);
|
||||
|
||||
/**
|
||||
* @brief Reset PID controllers and slew rates
|
||||
*/
|
||||
void resetControllers();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Compute normalized forward speed setpoint by applying slew rates
|
||||
* to the forward speed setpoint and doing closed loop speed control if enabled.
|
||||
* @param forward_speed_setpoint Forward speed setpoint [m/s].
|
||||
* @param vehicle_forward_speed Actual forward speed [m/s].
|
||||
* @param dt Time since last update [s].
|
||||
* @param normalized Indicates if the forward speed setpoint is already normalized.
|
||||
* @return Normalized forward speed setpoint with applied slew rates [-1, 1].
|
||||
*/
|
||||
float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float dt, bool normalized);
|
||||
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _rover_ackermann_setpoint_sub{ORB_ID(rover_ackermann_setpoint)};
|
||||
rover_ackermann_setpoint_s _rover_ackermann_setpoint{};
|
||||
|
||||
// uORB publications
|
||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
|
||||
uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};
|
||||
rover_ackermann_status_s _rover_ackermann_status{};
|
||||
|
||||
// Variables
|
||||
hrt_abstime _timestamp{0};
|
||||
|
||||
// Controllers
|
||||
PID_t _pid_throttle; // The PID controller for the closed loop speed control
|
||||
SlewRate<float> _steering_with_rate_limit{0.f};
|
||||
SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_p_speed,
|
||||
(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
|
||||
(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
|
||||
(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_THR_SPEED>) _param_ra_max_thr_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate,
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||
)
|
||||
};
|
||||
@@ -35,5 +35,4 @@ px4_add_library(RoverAckermannGuidance
|
||||
RoverAckermannGuidance.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(RoverAckermannGuidance PUBLIC pid)
|
||||
target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
@@ -42,35 +42,30 @@ RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModulePar
|
||||
{
|
||||
_rover_ackermann_guidance_status_pub.advertise();
|
||||
updateParams();
|
||||
pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||
}
|
||||
|
||||
void RoverAckermannGuidance::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
pid_set_parameters(&_pid_throttle,
|
||||
_param_ra_p_speed.get(), // Proportional gain
|
||||
_param_ra_i_speed.get(), // Integral gain
|
||||
0, // Derivative gain
|
||||
1, // Integral limit
|
||||
1); // Output limit
|
||||
|
||||
}
|
||||
|
||||
RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state)
|
||||
void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
|
||||
const float vehicle_yaw, const int nav_state, const bool armed)
|
||||
{
|
||||
updateSubscriptions();
|
||||
|
||||
// Distances to waypoints
|
||||
_distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_prev_wp(0), _prev_wp(1));
|
||||
_distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_curr_wp(0), _curr_wp(1));
|
||||
_distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_next_wp(0), _next_wp(1));
|
||||
const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_prev_wp(0), _prev_wp(1));
|
||||
const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
_curr_wp(0), _curr_wp(1));
|
||||
// const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
|
||||
// _next_wp(0), _next_wp(1));
|
||||
|
||||
// Catch return to launch
|
||||
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
|
||||
_mission_finished = _distance_to_next_wp < _acceptance_radius;
|
||||
_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
// Guidance logic
|
||||
@@ -78,39 +73,35 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
|
||||
_desired_steering = 0.f;
|
||||
_desired_speed = 0.f;
|
||||
|
||||
} else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command
|
||||
} else if (_nav_cmd == 93) { // Catch delay command
|
||||
_desired_speed = 0.f;
|
||||
|
||||
} else { // Regular guidance algorithm
|
||||
|
||||
_desired_speed = calcDesiredSpeed(_wp_max_desired_vel, _param_ra_miss_vel_min.get(),
|
||||
_param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius,
|
||||
_prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state);
|
||||
_desired_speed = calcDesiredSpeed(_cruising_speed, _param_ra_miss_spd_min.get(),
|
||||
_param_ra_miss_spd_gain.get(), distance_to_prev_wp, distance_to_curr_wp, _acceptance_radius,
|
||||
_prev_acceptance_radius, _param_ra_max_decel.get(), _param_ra_max_jerk.get(), nav_state, _waypoint_transition_angle,
|
||||
_prev_waypoint_transition_angle, _param_ra_max_speed.get());
|
||||
|
||||
_desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
|
||||
_param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get());
|
||||
_param_ra_wheel_base.get(), _desired_speed, vehicle_yaw, _param_ra_max_steer_angle.get(), armed);
|
||||
|
||||
}
|
||||
|
||||
// Calculate throttle setpoint
|
||||
hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(),
|
||||
dt);
|
||||
|
||||
// Publish ackermann controller status (logging)
|
||||
_rover_ackermann_guidance_status.timestamp = _timestamp;
|
||||
_rover_ackermann_guidance_status.desired_speed = _desired_speed;
|
||||
_rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral;
|
||||
hrt_abstime timestamp = hrt_absolute_time();
|
||||
_rover_ackermann_guidance_status.timestamp = timestamp;
|
||||
_rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status);
|
||||
|
||||
// Return motor setpoints
|
||||
motor_setpoint motor_setpoint_temp;
|
||||
motor_setpoint_temp.steering = math::interpolate<float>(_desired_steering, -_param_ra_max_steer_angle.get(),
|
||||
_param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint
|
||||
motor_setpoint_temp.throttle = throttle;
|
||||
return motor_setpoint_temp;
|
||||
// Publish speed and steering setpoints
|
||||
rover_ackermann_setpoint_s rover_ackermann_setpoint{};
|
||||
rover_ackermann_setpoint.timestamp = timestamp;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed;
|
||||
rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN;
|
||||
rover_ackermann_setpoint.steering_setpoint = _desired_steering;
|
||||
rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
|
||||
_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermannGuidance::updateSubscriptions()
|
||||
@@ -131,8 +122,6 @@ void RoverAckermannGuidance::updateSubscriptions()
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(local_position.x, local_position.y);
|
||||
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
|
||||
_actual_speed = rover_velocity.norm();
|
||||
}
|
||||
|
||||
if (_home_position_sub.updated()) {
|
||||
@@ -145,18 +134,17 @@ void RoverAckermannGuidance::updateSubscriptions()
|
||||
updateWaypointsAndAcceptanceRadius();
|
||||
}
|
||||
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_mission_result_sub.updated()) {
|
||||
mission_result_s mission_result{};
|
||||
_mission_result_sub.copy(&mission_result);
|
||||
_mission_finished = mission_result.finished;
|
||||
}
|
||||
|
||||
if (_navigator_mission_item_sub.updated()) {
|
||||
navigator_mission_item_s navigator_mission_item{};
|
||||
_navigator_mission_item_sub.copy(&navigator_mission_item);
|
||||
_nav_cmd = navigator_mission_item.nav_cmd;
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
|
||||
@@ -166,7 +154,7 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
|
||||
|
||||
// Global waypoint coordinates
|
||||
_prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
|
||||
_curr_wp = Vector2d(0, 0);
|
||||
_curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid
|
||||
_next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL
|
||||
|
||||
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
|
||||
@@ -192,46 +180,47 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
|
||||
_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
|
||||
_prev_waypoint_transition_angle = _waypoint_transition_angle;
|
||||
_waypoint_transition_angle = acosf(cosin);
|
||||
|
||||
// Update acceptance radius
|
||||
_prev_acceptance_radius = _acceptance_radius;
|
||||
|
||||
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
|
||||
_acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _param_nav_acc_rad.get(),
|
||||
_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
|
||||
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());
|
||||
|
||||
} else {
|
||||
_acceptance_radius = _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
// Waypoint cruising speed
|
||||
if (position_setpoint_triplet.current.cruising_speed > 0.f) {
|
||||
_wp_max_desired_vel = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get());
|
||||
_cruising_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get());
|
||||
|
||||
} else {
|
||||
_wp_max_desired_vel = _param_ra_miss_vel_def.get();
|
||||
_cruising_speed = _param_ra_miss_spd_def.get();
|
||||
}
|
||||
}
|
||||
|
||||
float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
|
||||
const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain,
|
||||
float RoverAckermannGuidance::updateAcceptanceRadius(const float waypoint_transition_angle,
|
||||
const float default_acceptance_radius, const float acceptance_radius_gain,
|
||||
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
|
||||
{
|
||||
// Setup variables
|
||||
const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
|
||||
const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned;
|
||||
float acceptance_radius = default_acceptance_radius;
|
||||
|
||||
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
|
||||
if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) {
|
||||
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
|
||||
const float theta = acosf(cosin) / 2.f;
|
||||
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
|
||||
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
|
||||
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
|
||||
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
|
||||
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
|
||||
acceptance_radius_max);
|
||||
}
|
||||
float acceptance_radius = default_acceptance_radius;
|
||||
const float theta = waypoint_transition_angle / 2.f;
|
||||
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
|
||||
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
|
||||
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
|
||||
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
|
||||
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
|
||||
acceptance_radius_max);
|
||||
|
||||
// Publish updated acceptance radius
|
||||
position_controller_status_s pos_ctrl_status{};
|
||||
@@ -241,51 +230,56 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned
|
||||
return acceptance_radius;
|
||||
}
|
||||
|
||||
float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min,
|
||||
const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
|
||||
const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state)
|
||||
float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const float miss_speed_min,
|
||||
const float miss_speed_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
|
||||
const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state,
|
||||
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
|
||||
{
|
||||
// Catch improper values
|
||||
if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) {
|
||||
return miss_vel_def;
|
||||
if (miss_speed_min < 0.f || miss_speed_min > cruising_speed || miss_speed_gain < FLT_EPSILON) {
|
||||
return cruising_speed;
|
||||
}
|
||||
|
||||
// Cornering slow down effect
|
||||
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
|
||||
const float cornering_speed = miss_vel_gain / prev_acc_rad;
|
||||
return math::constrain(cornering_speed, miss_vel_min, miss_vel_def);
|
||||
const float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - prev_waypoint_transition_angle,
|
||||
0.f, M_PI_F, 0.f, 1.f);
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
} else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) {
|
||||
const float cornering_speed = miss_vel_gain / acc_rad;
|
||||
return math::constrain(cornering_speed, miss_vel_min, miss_vel_def);
|
||||
const float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
|
||||
M_PI_F, 0.f, 1.f);
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
}
|
||||
|
||||
// Straight line speed
|
||||
if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
|
||||
float max_velocity{0.f};
|
||||
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
|
||||
float straight_line_speed{0.f};
|
||||
|
||||
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
|
||||
max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_accel, distance_to_curr_wp, 0.f);
|
||||
straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_decel, distance_to_curr_wp, 0.f);
|
||||
|
||||
} else {
|
||||
const float cornering_speed = miss_vel_gain / acc_rad;
|
||||
max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_accel, distance_to_curr_wp - acc_rad, cornering_speed);
|
||||
float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, M_PI_F,
|
||||
0.f, 1.f);
|
||||
cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
|
||||
}
|
||||
|
||||
return math::constrain(max_velocity, miss_vel_min, miss_vel_def);
|
||||
return math::min(straight_line_speed, cruising_speed);
|
||||
|
||||
} else {
|
||||
return miss_vel_def;
|
||||
return cruising_speed;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned,
|
||||
const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed,
|
||||
const float vehicle_yaw, const float max_steering)
|
||||
const float vehicle_yaw, const float max_steering, const bool armed)
|
||||
{
|
||||
const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
|
||||
desired_speed);
|
||||
@@ -297,6 +291,10 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con
|
||||
|
||||
float desired_steering{0.f};
|
||||
|
||||
if (!armed) {
|
||||
return desired_steering;
|
||||
}
|
||||
|
||||
if (math::abs_t(heading_error) <= M_PI_2_F) {
|
||||
desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
|
||||
|
||||
@@ -309,22 +307,3 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con
|
||||
return math::constrain(desired_steering, -max_steering, max_steering);
|
||||
|
||||
}
|
||||
|
||||
float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed,
|
||||
const float actual_speed, const float max_speed, const float dt)
|
||||
{
|
||||
float throttle = 0.f;
|
||||
|
||||
if (desired_speed < FLT_EPSILON) {
|
||||
pid_reset_integral(&pid_throttle);
|
||||
|
||||
} else {
|
||||
throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt);
|
||||
}
|
||||
|
||||
if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term
|
||||
throttle += math::interpolate<float>(desired_speed, 0.f, max_speed, 0.f, 1.f);
|
||||
}
|
||||
|
||||
return math::constrain(throttle, 0.f, 1.f);
|
||||
}
|
||||
|
||||
@@ -41,14 +41,15 @@
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/rover_ackermann_guidance_status.h>
|
||||
#include <uORB/topics/rover_ackermann_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/navigator_mission_item.h>
|
||||
|
||||
// Standard library includes
|
||||
#include <matrix/matrix/math.hpp>
|
||||
@@ -74,21 +75,13 @@ public:
|
||||
~RoverAckermannGuidance() = default;
|
||||
|
||||
/**
|
||||
* @brief Struct for steering and throttle setpoints.
|
||||
* @param steering Steering setpoint.
|
||||
* @param throttle Throttle setpoint.
|
||||
*/
|
||||
struct motor_setpoint {
|
||||
float steering{0.f};
|
||||
float throttle{0.f};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Calculate motor setpoints based on the mission plan.
|
||||
* @brief Compute and publish speed and steering setpoints based on the mission plan.
|
||||
* @param vehicle_forward_speed Forward speed of the vehicle [m/s]
|
||||
* @param vehicle_yaw Yaw of the vehicle [rad]
|
||||
* @param nav_state Vehicle navigation state.
|
||||
* @return Motor setpoints for throttle and steering.
|
||||
* @param armed Vehicle arm state.
|
||||
*/
|
||||
motor_setpoint computeGuidance(int nav_state);
|
||||
void computeGuidance(float vehicle_forward_speed, float vehicle_yaw, int nav_state, bool armed);
|
||||
|
||||
protected:
|
||||
/**
|
||||
@@ -110,9 +103,7 @@ private:
|
||||
/**
|
||||
* @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
|
||||
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
|
||||
* @param curr_wp_ned Current waypoint in NED frame [m].
|
||||
* @param prev_wp_ned Previous waypoint in NED frame [m].
|
||||
* @param next_wp_ned Next waypoint in NED frame [m].
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param default_acceptance_radius Default acceptance radius for waypoints [m].
|
||||
* @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
|
||||
* @param acceptance_radius_max Maximum value for the acceptance radius [m].
|
||||
@@ -120,30 +111,34 @@ private:
|
||||
* @param max_steer_angle Rover maximum steer angle [rad].
|
||||
* @return Updated acceptance radius [m].
|
||||
*/
|
||||
float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
|
||||
const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain,
|
||||
float updateAcceptanceRadius(const float waypoint_transition_angle, float default_acceptance_radius,
|
||||
float acceptance_radius_gain,
|
||||
float acceptance_radius_max, float wheel_base, float max_steer_angle);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse
|
||||
* of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory
|
||||
* of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a speed trajectory
|
||||
* such that the rover will arrive at the next corner with the desired cornering speed under consideration of the
|
||||
* maximum acceleration and jerk.
|
||||
* @param miss_vel_def Default desired velocity for the rover during mission [m/s].
|
||||
* @param miss_vel_min Minimum desired velocity for the rover during mission [m/s].
|
||||
* @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-].
|
||||
* @param cruising_speed Cruising speed [m/s].
|
||||
* @param miss_speed_min Minimum speed setpoint [m/s].
|
||||
* @param miss_speed_gain Tuning parameter for the slow down effect during cornering [-].
|
||||
* @param distance_to_prev_wp Distance to the previous waypoint [m].
|
||||
* @param distance_to_curr_wp Distance to the current waypoint [m].
|
||||
* @param acc_rad Acceptance radius of the current waypoint [m].
|
||||
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
|
||||
* @param max_accel Maximum allowed acceleration for the rover [m/s^2].
|
||||
* @param max_jerk Maximum allowed jerk for the rover [m/s^3].
|
||||
* @param max_accel Maximum allowed acceleration [m/s^2].
|
||||
* @param max_jerk Maximum allowed jerk [m/s^3].
|
||||
* @param nav_state Current nav_state of the rover.
|
||||
* @return Speed setpoint for the rover [m/s].
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param max_speed Maximum speed setpoint [m/s]
|
||||
* @return Speed setpoint [m/s].
|
||||
*/
|
||||
float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp,
|
||||
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state);
|
||||
float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float miss_speed_gain, float distance_to_prev_wp,
|
||||
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state,
|
||||
float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
|
||||
|
||||
/**
|
||||
* @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to
|
||||
@@ -156,32 +151,23 @@ private:
|
||||
* @param desired_speed Desired speed for the rover [m/s].
|
||||
* @param vehicle_yaw Current yaw of the rover [rad].
|
||||
* @param max_steering Maximum steering angle of the rover [rad].
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param armed Vehicle arm status
|
||||
* @return Steering setpoint for the rover [rad].
|
||||
*/
|
||||
float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
|
||||
const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering);
|
||||
|
||||
/**
|
||||
* @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between
|
||||
* the desired/actual speed and a feedforward term based on the full throttle speed.
|
||||
* @param pid_throttle Reference to PID instance.
|
||||
* @param desired_speed Reference speed for the rover [m/s].
|
||||
* @param actual_speed Actual speed of the rover [m/s].
|
||||
* @param max_speed Rover speed at full throttle [m/s].
|
||||
* @param dt Time interval since last update [s].
|
||||
* @return Normalized throttle setpoint [0, 1].
|
||||
*/
|
||||
float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt);
|
||||
const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering, bool armed);
|
||||
|
||||
// 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 _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
||||
uORB::Subscription _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
|
||||
uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};
|
||||
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
|
||||
rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{};
|
||||
@@ -192,13 +178,10 @@ private:
|
||||
|
||||
// Rover variables
|
||||
float _desired_steering{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
float _desired_speed{0.f};
|
||||
float _actual_speed{0.f};
|
||||
Vector2d _curr_pos{};
|
||||
Vector2f _curr_pos_ned{};
|
||||
PID_t _pid_throttle;
|
||||
hrt_abstime _timestamp{0};
|
||||
bool _mission_finished{false};
|
||||
|
||||
// Waypoint variables
|
||||
Vector2d _home_position{};
|
||||
@@ -208,13 +191,12 @@ private:
|
||||
Vector2d _curr_wp{};
|
||||
Vector2d _prev_wp{};
|
||||
Vector2d _next_wp{};
|
||||
float _distance_to_prev_wp{0.f};
|
||||
float _distance_to_curr_wp{0.f};
|
||||
float _distance_to_next_wp{0.f};
|
||||
float _acceptance_radius{0.5f};
|
||||
float _prev_acceptance_radius{0.5f};
|
||||
float _wp_max_desired_vel{0.f};
|
||||
bool _mission_finished{false};
|
||||
float _cruising_speed{0.f};
|
||||
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
uint _nav_cmd{0};
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
@@ -222,14 +204,14 @@ private:
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
|
||||
(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
|
||||
(ParamFloat<px4::params::RA_MISS_VEL_MIN>) _param_ra_miss_vel_min,
|
||||
(ParamFloat<px4::params::RA_MISS_VEL_GAIN>) _param_ra_miss_vel_gain,
|
||||
(ParamFloat<px4::params::RA_MISS_SPD_DEF>) _param_ra_miss_spd_def,
|
||||
(ParamFloat<px4::params::RA_MISS_SPD_MIN>) _param_ra_miss_spd_min,
|
||||
(ParamFloat<px4::params::RA_MISS_SPD_GAIN>) _param_ra_miss_spd_gain,
|
||||
(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_p_speed,
|
||||
(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
|
||||
(ParamFloat<px4::params::RA_MAX_JERK>) _param_ra_max_jerk,
|
||||
(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
|
||||
(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
|
||||
)
|
||||
};
|
||||
|
||||
@@ -57,9 +57,9 @@ parameters:
|
||||
decimal: 2
|
||||
default: 2
|
||||
|
||||
RA_MISS_VEL_DEF:
|
||||
RA_MISS_SPD_DEF:
|
||||
description:
|
||||
short: Default rover velocity during a mission
|
||||
short: Default rover speed during a mission
|
||||
type: float
|
||||
unit: m/s
|
||||
min: 0
|
||||
@@ -68,11 +68,11 @@ parameters:
|
||||
decimal: 2
|
||||
default: 2
|
||||
|
||||
RA_MISS_VEL_MIN:
|
||||
RA_MISS_SPD_MIN:
|
||||
description:
|
||||
short: Minimum rover velocity during a mission
|
||||
short: Minimum rover speed during a mission
|
||||
long: |
|
||||
The velocity off the rover is reduced based on the corner it has to take
|
||||
The speed off the rover is reduced based on the corner it has to take
|
||||
to smooth the trajectory (Set to -1 to disable)
|
||||
type: float
|
||||
unit: m/s
|
||||
@@ -82,13 +82,13 @@ parameters:
|
||||
decimal: 2
|
||||
default: 1
|
||||
|
||||
RA_MISS_VEL_GAIN:
|
||||
RA_MISS_SPD_GAIN:
|
||||
description:
|
||||
short: Tuning parameter for the velocity reduction during cornering
|
||||
short: Tuning parameter for the speed reduction during cornering
|
||||
long: |
|
||||
The cornering speed is equal to the inverse of the acceptance radius
|
||||
of the WP multiplied with this factor.
|
||||
Lower value -> More velocity reduction during cornering.
|
||||
Lower value -> More speed reduction during cornering.
|
||||
type: float
|
||||
min: 0.05
|
||||
max: 100
|
||||
@@ -112,17 +112,32 @@ parameters:
|
||||
type: float
|
||||
min: 0
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
increment: 0.001
|
||||
decimal: 3
|
||||
default: 1
|
||||
|
||||
RA_MAX_SPEED:
|
||||
description:
|
||||
short: Maximum allowed speed
|
||||
long: |
|
||||
This is the maximum allowed speed setpoint when driving in a mode that uses
|
||||
closed loop speed control.
|
||||
type: float
|
||||
unit: m/s
|
||||
min: -1
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: -1
|
||||
|
||||
RA_MAX_THR_SPEED:
|
||||
description:
|
||||
short: Speed the rover drives at maximum throttle
|
||||
long: |
|
||||
This is used for the feed-forward term of the speed controller.
|
||||
A value of -1 disables the feed-forward term in which case the
|
||||
Integrator (RA_SPEED_I) becomes necessary to track speed setpoints.
|
||||
This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
|
||||
maps desired speeds to normalized motor commands [-1. 1].
|
||||
A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
|
||||
Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
|
||||
type: float
|
||||
unit: m/s
|
||||
min: -1
|
||||
@@ -147,6 +162,22 @@ parameters:
|
||||
decimal: 2
|
||||
default: -1
|
||||
|
||||
RA_MAX_DECEL:
|
||||
description:
|
||||
short: Maximum deceleration for the rover
|
||||
long: |
|
||||
This is used for the deceleration slew rate, the feed-forward term
|
||||
for the speed controller during missions and the corner slow down effect.
|
||||
Note: For the corner slow down effect RA_MAX_JERK, RA_MISS_VEL_GAIN and
|
||||
RA_MISS_VEL_MIN also have to be set.
|
||||
type: float
|
||||
unit: m/s^2
|
||||
min: -1
|
||||
max: 100
|
||||
increment: 0.01
|
||||
decimal: 2
|
||||
default: -1
|
||||
|
||||
RA_MAX_JERK:
|
||||
description:
|
||||
short: Maximum jerk
|
||||
|
||||
Reference in New Issue
Block a user