diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 271071eeac..c9aca4c310 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -15,7 +15,7 @@ ekf2 start & # # Start manual rover differential drive controller. # -# differential_drive_control start +differential_drive_control start # diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index d2e0d0593e..7b542f5a3a 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 899698d476..31c0240816 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -69,6 +69,7 @@ set(msg_files DebugKeyValue.msg DebugValue.msg DebugVect.msg + DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg new file mode 100644 index 0000000000..386dc3a9d9 --- /dev/null +++ b/msg/DifferentialDriveSetpoint.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 speed # [m/s] collective roll-off speed in body x-axis +float32 yaw_rate # [rad/s] yaw rate diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 994b566270..2406b81bf8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,8 +62,6 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp - ActuatorEffectivenessRoverDifferential.hpp - ActuatorEffectivenessRoverDifferential.cpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 0fad1b5f62..a7b2af8998 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -61,6 +61,7 @@ ControlAllocator::ControlAllocator() : _actuator_servos_pub.advertise(); _actuator_servos_trim_pub.advertise(); + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { char buffer[17]; snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); @@ -239,7 +240,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - tmp = new ActuatorEffectivenessRoverDifferential(); + // actuator_motors message is published directly from the differential drive controller break; case EffectivenessSource::FIXED_WING: diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 25904f0551..303316f1ef 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/differential_drive_control/CMakeLists.txt b/src/modules/differential_drive_control/CMakeLists.txt new file mode 100644 index 0000000000..7fd6f1d8f3 --- /dev/null +++ b/src/modules/differential_drive_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +add_subdirectory(DifferentialDriveKinematics) + +px4_add_module( + MODULE modules__differential_drive_control + MAIN differential_drive_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + DifferentialDriveControl.cpp + DifferentialDriveControl.hpp + DEPENDS + DifferentialDriveKinematics + px4_work_queue +) diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/differential_drive_control/DifferentialDriveControl.cpp new file mode 100644 index 0000000000..a8b2ede499 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveControl.cpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "DifferentialDriveControl.hpp" + +using namespace time_literals; + +namespace differential_drive_control +{ + +DifferentialDriveControl::DifferentialDriveControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + _outputs_pub.advertise(); + _last_timestamp = hrt_absolute_time(); + _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + _differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get()); +} + +int DifferentialDriveControl::task_spawn(int argc, char *argv[]) +{ + DifferentialDriveControl *obj = new DifferentialDriveControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + obj->start(); + + return 0; +} + +void DifferentialDriveControl::start() +{ + ScheduleOnInterval(10_ms); // 100 Hz +} + +void DifferentialDriveControl::Run() +{ + + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + _current_timestamp = hrt_absolute_time(); + + vehicle_control_mode_poll(); + + if (_parameter_update_sub.updated()) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + updateParams(); + + _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + _differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get()); + } + + if (_vehicle_control_mode.flag_armed) { + + if (_manual_control_setpoint_sub.updated()) { + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + // Manual mode + if (_vehicle_control_mode.flag_control_manual_enabled) { + // directly get the input from the manual control setpoint (joystick) + + _differential_drive_setpoint.timestamp = hrt_absolute_time(); + + _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_max_speed.get(); + _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_max_angular_velocity.get(); + + _differential_drive_setpoint_pub.publish(_differential_drive_setpoint); + } + } + + // if manual is not enabled, it will look for a differential drive setpoint topic (differential_drive_setpoint) and subscribe to it from a ROS2 mode over DDS + if (_differential_drive_setpoint_sub.updated()) { + _differential_drive_setpoint_sub.copy(&_differential_drive_setpoint); + + _velocity_control_inputs(0) = _differential_drive_setpoint.speed; + _velocity_control_inputs(1) = _differential_drive_setpoint.yaw_rate; + + } else if (_differential_drive_setpoint.timestamp <= 100_ms) { + _velocity_control_inputs(0) = 0.0f; + _velocity_control_inputs(1) = 0.0f; + } + + } else { + // if the system is in an error state, stop the vehicle + _velocity_control_inputs = {0.0f, 0.0f}; + } + + // publish data to actuator_motors (output module) + publishWheelControl(); + + _last_timestamp = _current_timestamp; + +} + +void DifferentialDriveControl::publishWheelControl() +{ + // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) + _output_inverse = _differential_drive_kinematics.computeInverseKinematics(_velocity_control_inputs(0), + _velocity_control_inputs(1)); + + // Superpose Linear and Angular velocity vector + float max_angular_wheel_speed = ((_param_rdd_max_speed.get() + (_param_rdd_max_angular_velocity.get() * + _param_rdd_wheel_base.get() / 2.f)) / _param_rdd_wheel_radius.get()); + + // Check if max_angular_wheel_speed is zero + if (fabsf(max_angular_wheel_speed) < FLT_EPSILON) { + // Guard against division by zero + max_angular_wheel_speed = 0.001f; + } + + _actuator_motors.timestamp = hrt_absolute_time(); + + // bitset which motors are configured to be reversible (3 -> ..00 0000 0011 this means that the first two motors are reversible) + _actuator_motors.reversible_flags = 3; + _actuator_motors.control[0] = _output_inverse(0) / max_angular_wheel_speed; + _actuator_motors.control[1] = _output_inverse(1) / max_angular_wheel_speed; + + _outputs_pub.publish(_actuator_motors); +} + +void DifferentialDriveControl::vehicle_control_mode_poll() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } +} + +int DifferentialDriveControl::print_status() +{ + PX4_INFO("Diffential Drive Controller running"); + return 0; +} + +int DifferentialDriveControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Differential Drive controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("differential_drive", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[]) +{ + return DifferentialDriveControl::main(argc, argv); +} + +} // namespace differential_drive_control diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.hpp b/src/modules/differential_drive_control/DifferentialDriveControl.hpp new file mode 100644 index 0000000000..9364bd6e82 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveControl.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Cruise mode +#include +#include + +// Standard library includes +#include + +// Local includes +#include + +namespace differential_drive_control +{ + +class DifferentialDriveControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + DifferentialDriveControl(); + ~DifferentialDriveControl() override = default; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void start(); + +private: + + void Run() override; + void publishWheelControl(); + void vehicle_control_mode_poll(); + + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_motors)}; + uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_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 _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + + differential_drive_setpoint_s _differential_drive_setpoint{}; + vehicle_control_mode_s _vehicle_control_mode{}; + actuator_motors_s _actuator_motors{}; + + DifferentialDriveKinematics _differential_drive_kinematics; + + matrix::Vector2f _velocity_control_inputs{0.f, 0.f}; // [m/s] collective roll-off speed in body x-axis, [rad/s] yaw rate + matrix::Vector2f _output_inverse{0.0f, 0.0f}; // [rad/s] Right Motor, [rad/s] Left Motor + + float _last_timestamp{0.f}; + float _current_timestamp{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_max_speed, + (ParamFloat) _param_rdd_max_angular_velocity, + (ParamFloat) _param_rdd_wheel_base, + (ParamFloat) _param_rdd_wheel_radius + ) +}; + +} // namespace differential_drive_control diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt new file mode 100644 index 0000000000..2ba1fc7b86 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2023 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(DifferentialDriveKinematics + DifferentialDriveKinematics.cpp + DifferentialDriveKinematics.hpp +) + +target_compile_options(DifferentialDriveKinematics PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp similarity index 67% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp rename to src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp index fed0a7ddcf..2e1cd6a3b7 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2023 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 @@ -31,23 +31,22 @@ * ****************************************************************************/ -#pragma once +#include "DifferentialDriveKinematics.hpp" -#include "ActuatorEffectiveness.hpp" - -class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness +void DifferentialDriveKinematics::setWheelBase(float wheel_base) { -public: - ActuatorEffectivenessRoverDifferential() = default; - virtual ~ActuatorEffectivenessRoverDifferential() = default; + _wheel_base = wheel_base; +} - bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; +void DifferentialDriveKinematics::setWheelRadius(float wheel_radius) +{ + _wheel_radius = wheel_radius; +} - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; +matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_vel_x, float yaw_rate) +{ + float motor_vel_right = linear_vel_x / _wheel_radius - _wheel_base / 2.f * yaw_rate / _wheel_radius; + float motor_vel_left = linear_vel_x / _wheel_radius + _wheel_base / 2.f * yaw_rate / _wheel_radius; - const char *name() const override { return "Rover (Differential)"; } -private: - uint32_t _motors_mask{}; -}; + return matrix::Vector2f(motor_vel_right, motor_vel_left); +} diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp new file mode 100644 index 0000000000..f25c18ee66 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +#include + +/** + * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. + * + * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics + * given linear velocity and yaw rate. + */ +class DifferentialDriveKinematics +{ +public: + DifferentialDriveKinematics() = default; + ~DifferentialDriveKinematics() = default; + + /** + * @brief Computes the inverse kinematics for differential drive. + * + * @param linear_vel_x Linear velocity along the x-axis. + * @param yaw_rate Yaw rate of the robot. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeInverseKinematics(float linear_vel_x, float yaw_rate); + + /** + * @brief Sets the wheel base of the robot. + * + * @param wheel_base The distance between the centers of the wheels. + */ + void setWheelBase(float wheel_base); + + /** + * @brief Sets the radius of the wheels of the robot. + * + * @param wheel_radius The radius of the wheels. + */ + void setWheelRadius(float wheel_radius); + +private: + float _wheel_base{0.f}; + float _wheel_radius{0.f}; +}; diff --git a/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp new file mode 100644 index 0000000000..1b0e514938 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include "DifferentialDriveKinematics.hpp" +#include + +using namespace matrix; + +TEST(DifferentialDriveKinematicsTest, AllZeroCaseInverse) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setWheelRadius(1.f); + Vector2f rate_setpoint = {0.f, 0.f}; + kinematics.setInput(rate_setpoint, true); + Vector2f wheel_output = kinematics.getOutput(true); + EXPECT_EQ(wheel_output, Vector2f()); +} + +TEST(DifferentialDriveKinematicsTest, InvalidCaseInverse) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(0.f); + kinematics.setWheelRadius(0.f); + Vector2f rate_setpoint = {0.f, 0.f}; + kinematics.setInput(rate_setpoint, true); + Vector2f wheel_output = kinematics.getOutput(true); + EXPECT_EQ(wheel_output, Vector2f()); +} + +TEST(DifferentialDriveKinematicsTest, UnitCaseInverse) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setWheelRadius(1.f); + Vector2f rate_setpoint = {1.f, 1.f}; + kinematics.setInput(rate_setpoint, true); + Vector2f wheel_output = kinematics.getOutput(true); + Vector2f expected_output = {1.5f, 0.5f}; + EXPECT_EQ(wheel_output, expected_output); +} + +TEST(DifferentialDriveKinematicsTest, UnitCase) +{ + DifferentialDriveKinematics kinematics; + kinematics.setWheelBase(1.f); + kinematics.setWheelRadius(1.f); + Vector2f wheel_input = {1.f, 1.f}; + kinematics.setInput(wheel_input, false); + Vector2f rate_output = kinematics.getOutput(false); + Vector2f expected_output = {1.f, 0.f}; + EXPECT_EQ(rate_output, expected_output); +} diff --git a/src/modules/differential_drive_control/Kconfig b/src/modules/differential_drive_control/Kconfig new file mode 100644 index 0000000000..5fb5bbf319 --- /dev/null +++ b/src/modules/differential_drive_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL + bool "differential_drive_control" + default y + ---help--- + Enable support for differential_drive_control diff --git a/src/modules/differential_drive_control/module.yaml b/src/modules/differential_drive_control/module.yaml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/src/modules/differential_drive_control/params.c similarity index 61% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp rename to src/modules/differential_drive_control/params.c index 8e7c64baa8..670711a996 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp +++ b/src/modules/differential_drive_control/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 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 @@ -31,29 +31,54 @@ * ****************************************************************************/ -#include "ActuatorEffectivenessRoverDifferential.hpp" -#include +/** + * Wheel Base + * + * Distance from the center of the right wheel to the center of the left wheel + * + * @unit m + * @min 0.001 + * @max 100 + * @increment 0.001 + * @decimal 3 + * @group Rover Differential Drive + */ +PARAM_DEFINE_FLOAT(RDD_WHEEL_BASE, 0.5f); -using namespace matrix; +/** + * Wheel radius + * + * Size of the wheel, half the diameter of the wheel + * + * @unit m + * @min 0.001 + * @max 100 + * @increment 0.001 + * @decimal 3 + * @group Rover Differential Drive + */ +PARAM_DEFINE_FLOAT(RDD_WHEEL_RADIUS, 0.1f); -bool -ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, - EffectivenessUpdateReason external_update) -{ - if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { - return false; - } - - configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f}); - configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f}); - _motors_mask = (1u << 0) | (1u << 1); - return true; -} - -void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) -{ - stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); -} +/** + * Max Speed + * + * @unit m/s + * @min 0.0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Differential Drive + */ +PARAM_DEFINE_FLOAT(RDD_MAX_SPEED, 0.5f); +/** + * Max Angular Velocity + * + * @unit rad/s + * @min 0.0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Differential Drive + */ +PARAM_DEFINE_FLOAT(RDD_MAX_ANG_VEL, 0.3f);