From 45540455fe9ddae51af71e6ee129ffebe1d67921 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 21 Apr 2025 12:35:10 +0200 Subject: [PATCH] ackermann: separate actuator control --- .../AckermannActControl.cpp | 105 +++++++++++++++++ .../AckermannActControl.hpp | 111 ++++++++++++++++++ .../AckermannActControl/CMakeLists.txt | 38 ++++++ src/modules/rover_ackermann/CMakeLists.txt | 2 + .../rover_ackermann/RoverAckermann.cpp | 80 ++----------- .../rover_ackermann/RoverAckermann.hpp | 42 +------ 6 files changed, 265 insertions(+), 113 deletions(-) create mode 100644 src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp new file mode 100644 index 0000000000..c898186ec7 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "AckermannActControl.hpp" + +using namespace time_literals; + +AckermannActControl::AckermannActControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void AckermannActControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); + } + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } +} + +void AckermannActControl::updateActControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + generateActuatorSetpoint(); + +} + +void AckermannActControl::generateActuatorSetpoint() +{ + // Motor control + rover_throttle_setpoint_s rover_throttle_setpoint{}; + _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); + actuator_motors_s actuator_motors_sub{}; + _actuator_motors_sub.copy(&actuator_motors_sub); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, + rover_throttle_setpoint.throttle_body_x, actuator_motors_sub.control[0], _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + // Servo control + rover_steering_setpoint_s rover_steering_setpoint{}; + _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); + actuator_servos_s actuator_servos_sub{}; + _actuator_servos_sub.copy(&actuator_servos_sub); + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf( + rover_steering_setpoint.normalized_steering_angle - + actuator_servos_sub.control[0])) { + _servo_setpoint.setForcedValue(actuator_servos_sub.control[0]); + } + + _servo_setpoint.update(rover_steering_setpoint.normalized_steering_angle, _dt); + + } else { + _servo_setpoint.setForcedValue(rover_steering_setpoint.normalized_steering_angle); + } + + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = _servo_setpoint.getState(); + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); +} diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp new file mode 100644 index 0000000000..d8933525e3 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// Libraries +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann actuator control. + */ +class AckermannActControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannActControl. + * @param parent The parent ModuleParams object. + */ + AckermannActControl(ModuleParams *parent); + ~AckermannActControl() = default; + + /** + * @brief Update actuator controller. + */ + void updateActControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + // uORB subscriptions + uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + + // Variables + hrt_abstime _timestamp{0}; + float _dt{0.f}; + + // Controllers + SlewRate _servo_setpoint{0.f}; + SlewRate _motor_setpoint{0.f}; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_str_rate_limit, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed + ) +}; diff --git a/src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt new file mode 100644 index 0000000000..59c168320c --- /dev/null +++ b/src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 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(AckermannActControl + AckermannActControl.cpp +) + +target_include_directories(AckermannActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 957f56d74b..8f71478554 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(AckermannActControl) add_subdirectory(AckermannRateControl) add_subdirectory(AckermannAttControl) add_subdirectory(AckermannVelControl) @@ -43,6 +44,7 @@ px4_add_module( RoverAckermann.cpp RoverAckermann.hpp DEPENDS + AckermannActControl AckermannRateControl AckermannAttControl AckermannVelControl diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index c586741e25..d57d6a0d8c 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -53,14 +53,6 @@ bool RoverAckermann::init() void RoverAckermann::updateParams() { ModuleParams::updateParams(); - - if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { - _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); - } - - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); - } } void RoverAckermann::Run() @@ -69,15 +61,6 @@ void RoverAckermann::Run() updateParams(); } - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - _ackermann_pos_control.updatePosControl(); - _ackermann_vel_control.updateVelControl(); - _ackermann_att_control.updateAttControl(); - _ackermann_rate_control.updateRateControl(); - if (_vehicle_control_mode_sub.updated()) { _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } @@ -90,8 +73,12 @@ void RoverAckermann::Run() generateSteeringAndThrottleSetpoint(); } - generateActuatorSetpoint(); + _ackermann_pos_control.updatePosControl(); + _ackermann_vel_control.updateVelControl(); + _ackermann_att_control.updateAttControl(); + _ackermann_rate_control.updateRateControl(); + _ackermann_act_control.updateActControl(); } void RoverAckermann::generateSteeringAndThrottleSetpoint() @@ -100,70 +87,17 @@ void RoverAckermann::generateSteeringAndThrottleSetpoint() if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.timestamp = hrt_absolute_time(); rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll; _rover_steering_setpoint_pub.publish(rover_steering_setpoint); rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.timestamp = hrt_absolute_time(); rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; rover_throttle_setpoint.throttle_body_y = 0.f; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } -void RoverAckermann::generateActuatorSetpoint() -{ - if (_rover_throttle_setpoint_sub.updated()) { - _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); - } - - if (_actuator_motors_sub.updated()) { - actuator_motors_s actuator_motors{}; - _actuator_motors_sub.copy(&actuator_motors); - _current_motor_setpoint = actuator_motors.control[0]; - } - - if (_vehicle_control_mode.flag_armed) { - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, - _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - } - - if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); - } - - if (_actuator_servos_sub.updated()) { - actuator_servos_s actuator_servos{}; - _actuator_servos_sub.copy(&actuator_servos); - _current_servo_setpoint = actuator_servos.control[0]; - } - - if (_param_ra_str_rate_limit.get() > FLT_EPSILON - && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured - if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf( - _rover_steering_setpoint.normalized_steering_angle - - _current_servo_setpoint)) { - _servo_setpoint.setForcedValue(_current_servo_setpoint); - } - - _servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt); - - } else { - _servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle); - } - - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = _servo_setpoint.getState(); - actuator_servos.timestamp = _timestamp; - _actuator_servos_pub.publish(actuator_servos); -} - int RoverAckermann::task_spawn(int argc, char *argv[]) { RoverAckermann *instance = new RoverAckermann(); diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 1a1cd0c987..0655ba9dda 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -40,24 +40,17 @@ #include #include -// Libraries -#include -#include -#include - // uORB includes #include #include -#include #include -#include -#include #include #include #include #include // Local includes +#include "AckermannActControl/AckermannActControl.hpp" #include "AckermannRateControl/AckermannRateControl.hpp" #include "AckermannAttControl/AckermannAttControl.hpp" #include "AckermannVelControl/AckermannVelControl.hpp" @@ -98,52 +91,21 @@ private: */ void generateSteeringAndThrottleSetpoint(); - /** - * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. - */ - void generateActuatorSetpoint(); - // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; - uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; vehicle_control_mode_s _vehicle_control_mode{}; - rover_steering_setpoint_s _rover_steering_setpoint{}; - rover_throttle_setpoint_s _rover_throttle_setpoint{}; // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances + AckermannActControl _ackermann_act_control{this}; AckermannRateControl _ackermann_rate_control{this}; AckermannAttControl _ackermann_att_control{this}; AckermannVelControl _ackermann_vel_control{this}; AckermannPosControl _ackermann_pos_control{this}; - // Variables - hrt_abstime _timestamp{0}; - float _dt{0.f}; - float _current_servo_setpoint{0.f}; - float _current_motor_setpoint{0.f}; - - // Controllers - SlewRate _servo_setpoint{0.f}; - SlewRate _motor_setpoint{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ra_str_rate_limit, - (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed - ) };