diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 6f81b7036a..da33f6d245 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 24f70e12a6..c8c8518bb8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -185,6 +185,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg + RoverAckermannSetpoint.msg RoverAckermannStatus.msg RoverDifferentialGuidanceStatus.msg RoverDifferentialSetpoint.msg diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg index a06d1290bf..bc096ba4d6 100644 --- a/msg/RoverAckermannGuidanceStatus.msg +++ b/msg/RoverAckermannGuidanceStatus.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 diff --git a/msg/RoverAckermannSetpoint.msg b/msg/RoverAckermannSetpoint.msg new file mode 100644 index 0000000000..c0d368bcce --- /dev/null +++ b/msg/RoverAckermannSetpoint.msg @@ -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 diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg index bf0556b945..92f29c60a8 100644 --- a/msg/RoverAckermannStatus.msg +++ b/msg/RoverAckermannStatus.msg @@ -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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8bd55e62fe..3ff1c321e7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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); diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index f02a91c0b6..423b643bdc 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -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 diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5409e40d84..03985586b7 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -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[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 3617b5b80b..7e18e2a723 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -39,27 +39,25 @@ #include #include #include -#include // uORB includes #include -#include #include #include #include #include -#include -#include -#include +#include #include +#include // Standard library includes #include +#include // 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_pub{ORB_ID(actuator_motors)}; - uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; - uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; + uORB::Publication _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 _steering_with_rate_limit{0.f}; - SlewRate _throttle_with_accel_limit{0.f}; + float _vehicle_forward_speed{0.f}; + float _vehicle_yaw{0.f}; bool _armed{false}; - // Parameters - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_accel, - (ParamFloat) _param_ra_max_steering_rate - ) }; diff --git a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt new file mode 100644 index 0000000000..9645c7acf9 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt @@ -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}) diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp new file mode 100644 index 0000000000..0adf5974a7 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp @@ -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 + +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(_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(_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); +} diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp new file mode 100644 index 0000000000..789fbe0267 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp @@ -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 + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +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_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _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 _steering_with_rate_limit{0.f}; + SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ra_p_speed, + (ParamFloat) _param_ra_i_speed, + (ParamFloat) _param_ra_max_accel, + (ParamFloat) _param_ra_max_decel, + (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_ra_max_thr_speed, + (ParamFloat) _param_ra_max_steer_angle, + (ParamFloat) _param_ra_max_steering_rate, + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt index 72928c7e25..373fc9b1be 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt @@ -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}) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index 48890323bb..6b6e6b091e 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -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(_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(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(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(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(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(desired_speed, 0.f, max_speed, 0.f, 1.f); - } - - return math::constrain(throttle, 0.f, 1.f); -} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index de5ef58780..eadb06d8f3 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -41,14 +41,15 @@ #include #include #include +#include #include #include #include -#include #include #include #include #include +#include // Standard library includes #include @@ -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_pub{ORB_ID(rover_ackermann_setpoint)}; uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; uORB::Publication _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) _param_ra_max_steer_angle, (ParamFloat) _param_ra_acc_rad_max, (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ra_miss_vel_def, - (ParamFloat) _param_ra_miss_vel_min, - (ParamFloat) _param_ra_miss_vel_gain, + (ParamFloat) _param_ra_miss_spd_def, + (ParamFloat) _param_ra_miss_spd_min, + (ParamFloat) _param_ra_miss_spd_gain, (ParamFloat) _param_ra_p_speed, (ParamFloat) _param_ra_i_speed, (ParamFloat) _param_ra_max_speed, (ParamFloat) _param_ra_max_jerk, - (ParamFloat) _param_ra_max_accel, + (ParamFloat) _param_ra_max_decel, (ParamFloat) _param_nav_acc_rad ) }; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index 6571d0eeeb..7d61613c01 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -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