From ac80958cc5d882439cc596cc3015d7c657a23f15 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 6 May 2025 09:35:42 +0200 Subject: [PATCH] differential: seperate actuator control --- src/modules/rover_differential/CMakeLists.txt | 2 + .../DifferentialActControl/CMakeLists.txt | 38 ++++++ .../DifferentialActControl.cpp | 125 ++++++++++++++++++ .../DifferentialActControl.hpp | 121 +++++++++++++++++ .../rover_differential/RoverDifferential.cpp | 74 +---------- .../rover_differential/RoverDifferential.hpp | 57 +------- 6 files changed, 292 insertions(+), 125 deletions(-) create mode 100644 src/modules/rover_differential/DifferentialActControl/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp create mode 100644 src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index a850e5949c..ebae1e8723 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(DifferentialActControl) add_subdirectory(DifferentialRateControl) add_subdirectory(DifferentialAttControl) add_subdirectory(DifferentialVelControl) @@ -43,6 +44,7 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS + DifferentialActControl DifferentialRateControl DifferentialAttControl DifferentialVelControl diff --git a/src/modules/rover_differential/DifferentialActControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialActControl/CMakeLists.txt new file mode 100644 index 0000000000..b3ab8187bd --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/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(DifferentialActControl + DifferentialActControl.cpp +) + +target_include_directories(DifferentialActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp new file mode 100644 index 0000000000..dd9c8c99e0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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 "DifferentialActControl.hpp" + +using namespace time_literals; + +DifferentialActControl::DifferentialActControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void DifferentialActControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _adjusted_throttle_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } +} + +void DifferentialActControl::updateActControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // Motor control + if (_rover_throttle_setpoint_sub.updated()) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); + _throttle_setpoint = rover_throttle_setpoint.throttle_body_x; + } + + if (_rover_steering_setpoint_sub.updated()) { + rover_steering_setpoint_s rover_steering_setpoint{}; + _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); + _speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff; + } + + if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) { + actuator_motors_s actuator_motors_sub{}; + _actuator_motors_sub.copy(&actuator_motors_sub); + const float current_throttle = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; + const float adjusted_throttle_setpoint = RoverControl::throttleControl(_adjusted_throttle_setpoint, + _throttle_setpoint, current_throttle, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(adjusted_throttle_setpoint, _speed_diff_setpoint).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + } + +} + +Vector2f DifferentialActControl::computeInverseKinematics(float throttle, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle -= sign(throttle) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle - speed_diff_normalized, + throttle + speed_diff_normalized); +} + +void DifferentialActControl::manualManualMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = hrt_absolute_time(); + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + 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 DifferentialActControl::stopVehicle() +{ + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = 0.f; + actuator_motors.control[1] = 0.f; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp new file mode 100644 index 0000000000..ff893fbfbb --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 + +/** + * @brief Class for differential actuator control. + */ +class DifferentialActControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialActControl. + * @param parent The parent ModuleParams object. + */ + DifferentialActControl(ModuleParams *parent); + ~DifferentialActControl() = default; + + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void updateActControl(); + + /** + * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + */ + void manualManualMode(); + + /** + * @brief Stop the vehicle by sending 0 commands to motors and servos. + */ + void stopVehicle(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle, float speed_diff_normalized); + + // uORB subscriptions + 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::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + // uORB publications + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + + // Variables + hrt_abstime _timestamp{0}; + float _throttle_setpoint{NAN}; + float _speed_diff_setpoint{NAN}; + + // Controllers + SlewRate _adjusted_throttle_setpoint{0.f}; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed + ) +}; diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 9a2a71646a..99dd28d55f 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -39,8 +39,6 @@ RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -53,10 +51,6 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); - } } void RoverDifferential::Run() @@ -65,10 +59,6 @@ void RoverDifferential::Run() updateParams(); } - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - _differential_pos_control.updatePosControl(); _differential_vel_control.updateVelControl(); _differential_att_control.updateAttControl(); @@ -83,74 +73,18 @@ void RoverDifferential::Run() && !_vehicle_control_mode.flag_control_rates_enabled; if (full_manual_mode_enabled) { // Manual mode - generateSteeringAndThrottleSetpoint(); + _differential_act_control.manualManualMode(); } if (_vehicle_control_mode.flag_armed) { - generateActuatorSetpoint(); + _differential_act_control.updateActControl(); + } else { + _differential_act_control.stopVehicle(); } } -void RoverDifferential::generateSteeringAndThrottleSetpoint() -{ - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_speed_diff = 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.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } -} - -void RoverDifferential::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_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; - } - - if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); - } - - const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, - _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(throttle_body_x, - _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) -{ - float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - throttle_body_x -= sign(throttle_body_x) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(throttle_body_x - speed_diff_normalized, - throttle_body_x + speed_diff_normalized); -} - int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 5a98ef7d3c..73fb3e735d 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -40,22 +40,13 @@ #include #include -// Libraries -#include -#include - // uORB includes #include -#include -#include #include -#include -#include -#include #include -#include // Local includes +#include "DifferentialActControl/DifferentialActControl.hpp" #include "DifferentialRateControl/DifferentialRateControl.hpp" #include "DifferentialAttControl/DifferentialAttControl.hpp" #include "DifferentialVelControl/DifferentialVelControl.hpp" @@ -91,59 +82,15 @@ protected: private: void Run() override; - /** - * @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode). - */ - void generateSteeringAndThrottleSetpoint(); - - /** - * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. - */ - void generateActuatorSetpoint(); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param throttle_body_x Normalized speed in body x direction [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return Motor speeds for the right and left motors [-1, 1]. - */ - Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); - // 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_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 _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances + DifferentialActControl _differential_act_control{this}; DifferentialRateControl _differential_rate_control{this}; DifferentialAttControl _differential_att_control{this}; DifferentialVelControl _differential_vel_control{this}; DifferentialPosControl _differential_pos_control{this}; - - // Variables - hrt_abstime _timestamp{0}; - float _dt{0.f}; - float _current_throttle_body_x{0.f}; - - // Controllers - SlewRate _throttle_body_x_setpoint{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed - ) };