diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_r1_rover_mecanum new file mode 100644 index 0000000000..69c43165f3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_r1_rover_mecanum @@ -0,0 +1,41 @@ +#!/bin/sh +# @name Aion Robotics R1 Rover +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_differential_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# Simulated sensors +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +# Actuator mapping +param set-default SIM_GZ_WH_FUNC1 101 # right wheel front +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 + +param set-default SIM_GZ_WH_FUNC2 102 # left wheel front +param set-default SIM_GZ_WH_MIN2 0 +param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_DIS2 100 + +param set-default SIM_GZ_WH_FUNC3 103 # right wheel back +param set-default SIM_GZ_WH_MIN3 0 +param set-default SIM_GZ_WH_MAX3 200 +param set-default SIM_GZ_WH_DIS3 100 + +param set-default SIM_GZ_WH_FUNC4 104 # left wheel back +param set-default SIM_GZ_WH_MIN4 0 +param set-default SIM_GZ_WH_MAX4 200 +param set-default SIM_GZ_WH_DIS4 100 + +# param set-default SIM_GZ_WH_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index ed460d9929..453e1af306 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -82,6 +82,7 @@ px4_add_romfs_files( 4008_gz_advanced_plane 4009_gz_r1_rover 4010_gz_x500_mono_cam + 4011_gz_r1_rover_mecanum 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 848527a9e9..1fb0b24857 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -5,7 +5,7 @@ ekf2 start & # Start rover differential drive controller. -differential_drive start +mecanum_drive start # Start Land Detector. land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index e0cace3a32..995727a3d6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -6,6 +6,6 @@ set VEHICLE_TYPE rover_differential param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER param set-default CA_AIRFRAME 6 # Rover (Differential) -param set-default CA_R_REV 3 # Right and left motors reversible +param set-default CA_R_REV 15 # Right and left motors reversible param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a..c24ac49e02 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MECANUM_DRIVE=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index eaad5510bb..df705725ea 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -137,6 +137,7 @@ set(msg_files ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg + MecanumDriveSetpoint.msg MessageFormatRequest.msg MessageFormatResponse.msg Mission.msg diff --git a/msg/MecanumDriveSetpoint.msg b/msg/MecanumDriveSetpoint.msg new file mode 100644 index 0000000000..54a9ae94bc --- /dev/null +++ b/msg/MecanumDriveSetpoint.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32[2] speed # [m/s] collective roll-off speed in body x- and y-axis +bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward +float32 yaw_rate # [rad/s] yaw rate +bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward + +# TOPICS mecanum_drive_setpoint mecanum_drive_control_output diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index dc5f791af2..09398f334a 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -437,12 +437,25 @@ bool MixingOutput::update() bool all_disabled = true; _reversible_mask = 0; + // printf("max num outputs: %d\n", _max_num_outputs); + for (int i = 0; i < _max_num_outputs; ++i) { + + // printf(" _functions[i]: %p\n", (void *)_functions[i]); + // printf("_function_assignment: %d\n", (int)_function_assignment[i]); + if (_functions[i]) { + + // printf("valid function\n"); + // printf("armed or prearmed allowprearmcontrol %d\n", _functions[i]->allowPrearmControl()); + // printf("prearmed %d\n", _armed.prearmed); + // printf("armed %d\n", _armed.armed); + // printf("nan\n"); all_disabled = false; if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) { outputs[i] = _functions[i]->value(_function_assignment[i]); + printf("outputs[i]: %f\n", (double)outputs[i]); } else { outputs[i] = NAN; @@ -471,6 +484,9 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat { bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode(); + // printf("outputs from limitAndUpdateOutputs: %f, %f, %f, %f\n", (double)outputs[0], (double)outputs[1], + // (double)outputs[2], (double)outputs[3]); + if (_armed.lockdown || _armed.manual_lockdown) { // overwrite outputs in case of lockdown with disarmed values for (size_t i = 0; i < _max_num_outputs; i++) { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d..af2eebcf77 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -961,10 +961,14 @@ mixer: actuators: - actuator_type: 'motor' instances: - - name: 'Right Motor' - position: [ 0, 1., 0 ] - - name: 'Left Motor' - position: [ 0, -1., 0 ] + - name: 'Right Motor Front' + position: [ 1, 1., 0 ] + - name: 'Left Motor Front' + position: [ 1, -1., 0 ] + - name: 'Right Motor Back' + position: [ -1, 1., 0 ] + - name: 'Left Motor Back' + position: [ -1, -1., 0 ] 7: # Motors (6DOF) actuators: diff --git a/src/modules/mecanum_drive/.vscode/c_cpp_properties.json b/src/modules/mecanum_drive/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..af5a8df28d --- /dev/null +++ b/src/modules/mecanum_drive/.vscode/c_cpp_properties.json @@ -0,0 +1,20 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/src/modules/mecanum_drive/.vscode/settings.json b/src/modules/mecanum_drive/.vscode/settings.json new file mode 100644 index 0000000000..593784e4f1 --- /dev/null +++ b/src/modules/mecanum_drive/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/src/modules/mecanum_drive/CMakeLists.txt b/src/modules/mecanum_drive/CMakeLists.txt new file mode 100644 index 0000000000..e7deda5d43 --- /dev/null +++ b/src/modules/mecanum_drive/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2023-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. +# +############################################################################ + +add_subdirectory(MecanumDriveControl) +# add_subdirectory(MecanumDriveGuidance) +add_subdirectory(MecanumDriveKinematics) + +px4_add_module( + MODULE modules__mecanum_drive + MAIN mecanum_drive + SRCS + MecanumDrive.cpp + MecanumDrive.hpp + DEPENDS + MecanumDriveControl + # MecanumDriveGuidance + MecanumDriveKinematics + px4_work_queue + modules__control_allocator # for parameter CA_R_REV + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/mecanum_drive/Kconfig b/src/modules/mecanum_drive/Kconfig new file mode 100644 index 0000000000..b4d6de6ebe --- /dev/null +++ b/src/modules/mecanum_drive/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_MECANUM_DRIVE + bool "mecanum_drive" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of mecanum drive rovers diff --git a/src/modules/mecanum_drive/MecanumDrive.cpp b/src/modules/mecanum_drive/MecanumDrive.cpp new file mode 100644 index 0000000000..1d16c0238a --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDrive.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 "MecanumDrive.hpp" + +MecanumDrive::MecanumDrive() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool MecanumDrive::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void MecanumDrive::updateParams() +{ + ModuleParams::updateParams(); + + _mecanum_drive_kinematics.setWheelBase(_param_md_wheel_base.get(), _param_md_wheel_base.get()); + _mecanum_drive_kinematics.setWheelRadius(_param_md_wheel_radius.get()); + + _max_speed = _param_md_wheel_speed.get() * _param_md_wheel_radius.get(); + // _mecanum_drive_guidance.setMaxSpeed(_max_speed); + _mecanum_drive_kinematics.setMaxSpeed(_max_speed, _max_speed); + + _max_angular_velocity = _max_speed / (_param_md_wheel_base.get() / 2.f); + // _mecanum_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); + _mecanum_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); +} + +void MecanumDrive::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; + _time_stamp_last = now; + + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode{}; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _mecanum_drive_kinematics.setArmed(vehicle_control_mode.flag_armed); + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; + _mission_driving = vehicle_control_mode.flag_control_auto_enabled; + } + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); + } + } + + if (_manual_driving) { + // Manual mode + // directly produce setpoints from the manual control setpoint (joystick) + if (_manual_control_setpoint_sub.updated()) { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + mecanum_drive_setpoint_s setpoint{}; + setpoint.yaw_rate = manual_control_setpoint.throttle * math::max(0.f, _param_md_speed_scale.get()) * _max_speed; + setpoint.speed[1] = manual_control_setpoint.yaw * math::max(0.f, _param_md_speed_scale.get()) * _max_speed; + setpoint.speed[0] = manual_control_setpoint.roll * _param_md_ang_velocity_scale.get() * _max_angular_velocity; + // setpoint.speed[0] = 0; + // setpoint.speed[1] = 0.0; + // setpoint.yaw_rate = 0; + + // if acro mode, we activate the yaw rate control + if (_acro_driving) { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = true; + + } else { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = false; + } + + setpoint.timestamp = now; + _mecanum_drive_setpoint_pub.publish(setpoint); + } + } + + } + + // else if (_mission_driving) { + // Mission mode + // directly receive setpoints from the guidance library + // _mecanum_drive_guidance.computeGuidance( + // _mecanum_drive_control.getVehicleYaw(), + // _mecanum_drive_control.getVehicleBodyYawRate(), + // dt + // ); + // } + + _mecanum_drive_control.control(dt); + _mecanum_drive_kinematics.allocate(); +} + +int MecanumDrive::task_spawn(int argc, char *argv[]) +{ + MecanumDrive *instance = new MecanumDrive(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MecanumDrive::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MecanumDrive::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Mecanum Drive controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mecanum_drive", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int mecanum_drive_main(int argc, char *argv[]) +{ + return MecanumDrive::main(argc, argv); +} diff --git a/src/modules/mecanum_drive/MecanumDrive.hpp b/src/modules/mecanum_drive/MecanumDrive.hpp new file mode 100644 index 0000000000..7fb23bd446 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDrive.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MecanumDriveControl/MecanumDriveControl.hpp" +// #include "MecanumDriveGuidance/MecanumDriveGuidance.hpp" +#include "MecanumDriveKinematics/MecanumDriveKinematics.hpp" + +using namespace time_literals; + +class MecanumDrive : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + MecanumDrive(); + ~MecanumDrive() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +protected: + void updateParams() override; + +private: + void Run() override; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Publication _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)}; + + bool _manual_driving = false; + bool _mission_driving = false; + bool _acro_driving = false; + hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + + // MecanumDriveGuidance _mecanum_drive_guidance{this}; + MecanumDriveControl _mecanum_drive_control{this}; + MecanumDriveKinematics _mecanum_drive_kinematics{this}; + + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_md_ang_velocity_scale, + (ParamFloat) _param_md_speed_scale, + (ParamFloat) _param_md_wheel_base, + (ParamFloat) _param_md_wheel_speed, + (ParamFloat) _param_md_wheel_radius + ) +}; diff --git a/src/modules/mecanum_drive/MecanumDriveControl/CMakeLists.txt b/src/modules/mecanum_drive/MecanumDriveControl/CMakeLists.txt new file mode 100644 index 0000000000..2429c52722 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveControl/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(MecanumDriveControl + MecanumDriveControl.cpp +) + +target_link_libraries(MecanumDriveControl PUBLIC pid) +target_include_directories(MecanumDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.cpp b/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.cpp new file mode 100644 index 0000000000..f57b30a590 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 "MecanumDriveControl.hpp" + +using namespace matrix; +using namespace time_literals; + +MecanumDriveControl::MecanumDriveControl(ModuleParams *parent) : ModuleParams(parent) +{ + pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void MecanumDriveControl::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_pid_angular_velocity, + _param_rdd_p_gain_angular_velocity.get(), // Proportional gain + _param_rdd_i_gain_angular_velocity.get(), // Integral gain + 0, // Derivative gain + 20, // Integral limit + 200); // Output limit + + pid_set_parameters(&_pid_speed, + _param_rdd_p_gain_speed.get(), // Proportional gain + _param_rdd_i_gain_speed.get(), // Integral gain + 0, // Derivative gain + 2, // Integral limit + 200); // Output limit +} + +void MecanumDriveControl::control(float dt) +{ + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + + if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + + if (_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); + } + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s; + } + } + + _mecanum_drive_setpoint_sub.update(&_mecanum_drive_setpoint); + + // PID to reach setpoint using control_output + mecanum_drive_setpoint_s mecanum_drive_control_output = _mecanum_drive_setpoint; + + if (_mecanum_drive_setpoint.closed_loop_speed_control) { + mecanum_drive_control_output.speed[0] += + pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[0], _vehicle_forward_speed, 0, dt); + mecanum_drive_control_output.speed[1] += + pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[1], _vehicle_forward_speed, 0, dt); + } + + if (_mecanum_drive_setpoint.closed_loop_yaw_rate_control) { + mecanum_drive_control_output.yaw_rate += + pid_calculate(&_pid_angular_velocity, _mecanum_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); + } + + + if (!_spooled_up) { + mecanum_drive_control_output.speed[0] = 0.0f; + mecanum_drive_control_output.speed[1] = 0.0f; + mecanum_drive_control_output.yaw_rate = 0.0f; + } + + mecanum_drive_control_output.timestamp = hrt_absolute_time(); + _mecanum_drive_control_output_pub.publish(mecanum_drive_control_output); +} diff --git a/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.hpp b/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.hpp new file mode 100644 index 0000000000..d167259298 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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. + * + ****************************************************************************/ + +/** + * @file MecanumDriveControl.hpp + * + * Controller for heading rate and forward speed. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MecanumDriveControl : public ModuleParams +{ +public: + MecanumDriveControl(ModuleParams *parent); + ~MecanumDriveControl() = default; + + void control(float dt); + float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } + float getVehicleYaw() const { return _vehicle_yaw; } + +protected: + void updateParams() override; + +private: + uORB::Subscription _mecanum_drive_setpoint_sub{ORB_ID(mecanum_drive_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _mecanum_drive_control_output_pub{ORB_ID(mecanum_drive_control_output)}; + + mecanum_drive_setpoint_s _mecanum_drive_setpoint{}; + + matrix::Quatf _vehicle_attitude_quaternion{}; + float _vehicle_yaw{0.f}; + + // States + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + + PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. + PID_t _pid_speed; ///< The PID controller for velocity. + + bool _spooled_up{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_p_gain_speed, + (ParamFloat) _param_rdd_i_gain_speed, + (ParamFloat) _param_rdd_p_gain_angular_velocity, + (ParamFloat) _param_rdd_i_gain_angular_velocity, + + (ParamFloat) _param_com_spoolup_time + ) +}; diff --git a/src/modules/mecanum_drive/MecanumDriveGuidance/CMakeLists.txt b/src/modules/mecanum_drive/MecanumDriveGuidance/CMakeLists.txt new file mode 100644 index 0000000000..f49d651d44 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveGuidance/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2023-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(MecanumDriveGuidance + MecanumDriveGuidance.cpp +) + +target_include_directories(MecanumDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.cpp b/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.cpp new file mode 100644 index 0000000000..97819e6d4c --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 "MecanumDriveGuidance.hpp" + +#include + +using namespace matrix; + +MecanumDriveGuidance::MecanumDriveGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + + pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void MecanumDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) +{ + if (_position_setpoint_triplet_sub.updated()) { + _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); + } + + if (_vehicle_global_position_sub.updated()) { + _vehicle_global_position_sub.copy(&_vehicle_global_position); + } + + matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); + matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); + matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); + + const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + current_waypoint(0), + current_waypoint(1)); + + float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), + current_waypoint(1)); + float heading_error = matrix::wrap_pi(desired_heading - yaw); + + if (_current_waypoint != current_waypoint) { + _currentState = GuidanceState::TURNING; + } + + // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. + if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } + + float desired_speed = 0.f; + + switch (_currentState) { + case GuidanceState::TURNING: + desired_speed = 0.f; + + if (fabsf(heading_error) < 0.05f) { + _currentState = GuidanceState::DRIVING; + } + + break; + + case GuidanceState::DRIVING: { + const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_md_max_jerk.get(), + _param_md_max_accel.get(), distance_to_next_wp, 0.0f); + _forwards_velocity_smoothing.updateDurations(max_velocity); + _forwards_velocity_smoothing.updateTraj(dt); + desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, + _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); + break; + } + + case GuidanceState::GOAL_REACHED: + // temporary till I find a better way to stop the vehicle + desired_speed = 0.f; + heading_error = 0.f; + angular_velocity = 0.f; + _desired_angular_velocity = 0.f; + break; + } + + // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. + float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, + dt) + heading_error; + + mecanum_drive_setpoint_s output{}; + output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); + output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); + output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; + output.timestamp = hrt_absolute_time(); + + _mecanum_drive_setpoint_pub.publish(output); + + _current_waypoint = current_waypoint; +} + +void MecanumDriveGuidance::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_heading_p_controller, + _param_md_p_gain_heading.get(), // Proportional gain + 0, // Integral gain + 0, // Derivative gain + 0, // Integral limit + 200); // Output limit + + _forwards_velocity_smoothing.setMaxJerk(_param_md_max_jerk.get()); + _forwards_velocity_smoothing.setMaxAccel(_param_md_max_accel.get()); + _forwards_velocity_smoothing.setMaxVel(_max_speed); +} diff --git a/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.hpp b/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.hpp new file mode 100644 index 0000000000..c794d8712e --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + TURNING, ///< The vehicle is currently turning. + DRIVING, ///< The vehicle is currently driving straight. + GOAL_REACHED ///< The vehicle has reached its goal. +}; + +/** + * @brief Class for mecanum drive guidance. + */ +class MecanumDriveGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumDriveGuidance. + * @param parent The parent ModuleParams object. + */ + MecanumDriveGuidance(ModuleParams *parent); + ~MecanumDriveGuidance() = default; + + /** + * @brief Compute guidance for the vehicle. + * @param global_pos The global position of the vehicle in degrees. + * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. + * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. + * @param vehicle_yaw The yaw orientation of the vehicle in radians. + * @param body_velocity The velocity of the vehicle in m/s. + * @param angular_velocity The angular velocity of the vehicle in rad/s. + * @param dt The time step in seconds. + */ + void computeGuidance(float yaw, float angular_velocity, float dt); + + /** + * @brief Set the maximum speed for the vehicle. + * @param max_speed The maximum speed in m/s. + * @return The set maximum speed in m/s. + */ + float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } + + + /** + * @brief Set the maximum angular velocity for the vehicle. + * @param max_angular_velocity The maximum angular velocity in rad/s. + * @return The set maximum angular velocity in rad/s. + */ + float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + + uORB::Publication _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)}; + + position_setpoint_triplet_s _position_setpoint_triplet{}; + vehicle_global_position_s _vehicle_global_position{}; + + GuidanceState _currentState; ///< The current state of guidance. + + float _desired_angular_velocity; ///< The desired angular velocity. + + float _max_speed; ///< The maximum speed. + float _max_angular_velocity; ///< The maximum angular velocity. + + matrix::Vector2d _current_waypoint; ///< The current waypoint. + + VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. + PositionSmoothing _position_smoothing; ///< The position smoothing. + + PID_t _heading_p_controller; ///< The PID controller for yaw rate. + + DEFINE_PARAMETERS( + (ParamFloat) _param_md_p_gain_heading, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_md_max_jerk, + (ParamFloat) _param_md_max_accel + ) +}; diff --git a/src/modules/mecanum_drive/MecanumDriveKinematics/CMakeLists.txt b/src/modules/mecanum_drive/MecanumDriveKinematics/CMakeLists.txt new file mode 100644 index 0000000000..c3d39c9f00 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveKinematics/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023-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(MecanumDriveKinematics + MecanumDriveKinematics.cpp +) + +target_include_directories(MecanumDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_functional_gtest(SRC MecanumDriveKinematicsTest.cpp LINKLIBS MecanumDriveKinematics) diff --git a/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.cpp b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.cpp new file mode 100644 index 0000000000..8812f0e5b0 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2023-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 "MecanumDriveKinematics.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +MecanumDriveKinematics::MecanumDriveKinematics(ModuleParams *parent) : ModuleParams(parent) +{} + +void MecanumDriveKinematics::allocate() +{ + hrt_abstime now = hrt_absolute_time(); + + if (_mecanum_drive_control_output_sub.updated()) { + _mecanum_drive_control_output_sub.copy(&_mecanum_drive_control_output); + } + + const bool setpoint_timeout = (_mecanum_drive_control_output.timestamp + 100_ms) < now; + + Vector4f wheel_speeds = + computeInverseKinematics(_mecanum_drive_control_output.speed[0], _mecanum_drive_control_output.speed[1], + _mecanum_drive_control_output.yaw_rate); + + // printf("wheel_speeds: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1), (double)wheel_speeds(2), + // (double)wheel_speeds(3)); + + if (!_armed || setpoint_timeout) { + wheel_speeds = {}; // stop + } + + wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); + + // printf("wheel_speeds after all checks: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1), + // (double)wheel_speeds(2), + // (double)wheel_speeds(3)); + + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); // should be 15 see rc.rover_mecanum_defaults + wheel_speeds.copyTo(actuator_motors.control); + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); +} + +matrix::Vector4f MecanumDriveKinematics::computeInverseKinematics(float linear_velocity_x, float linear_velocity_y, + float yaw_rate) const +{ + // if (_max_speed < FLT_EPSILON) { + // return Vector4f(); + // } + + linear_velocity_x = math::constrain(linear_velocity_x, -_vx_max, _vx_max); + linear_velocity_y = math::constrain(linear_velocity_y, -_vy_max, _vy_max); + yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); + + // Define input vector and matrix data + float input_data[3] = {linear_velocity_x, linear_velocity_y, yaw_rate}; + Matrix input(input_data); + + float m_data[12] = {1, -1, -(_lx + _ly), 1, 1, (_lx + _ly), 1, 1, -(_lx + _ly), 1, -1, (_lx + _ly)}; + Matrix m(m_data); + + // Perform matrix-vector multiplication + auto result = m * input; // result is Matrix + + // Precompute scale factor + const float scale = 1 / _r; + + // Scale the result by 1/_r + for (size_t i = 0; i < 4; ++i) { + result(i, 0) *= scale; // Efficiently use precomputed scale factor + } + + // Initialize Vector4f with the scaled results + Vector4f output(result(0, 0), result(1, 0), result(2, 0), result(3, 0)); + + // output = {0.1f, 0.5f, -0.5f, -0.1f}; + + // printf("output: %f, %f, %f, %f\n", (double)output(0), (double)output(1), (double)output(2), (double)output(3)); + + return output; +} diff --git a/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.hpp b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.hpp new file mode 100644 index 0000000000..1715113760 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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 + +#include +#include +#include +#include +#include +#include + +/** + * @brief Mecanum Drive Kinematics class for computing the kinematics of a mecanum 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 MecanumDriveKinematics : public ModuleParams +{ +public: + MecanumDriveKinematics(ModuleParams *parent); + ~MecanumDriveKinematics() = default; + + /** + * @brief Sets the wheel base of the robot. + * + * @param wheel_base The distance between the centers of the wheels. + */ + void setWheelBase(const float lx, const float ly) { _lx = lx; _ly = ly; }; + + /** + * @brief Sets the maximum speed of the robot. + * + * @param max_speed The maximum speed of the robot. + */ + void setMaxSpeed(const float vx_max, const float vy_max) { _vx_max = vx_max; _vy_max = vy_max; }; + + /** + * @brief Sets the maximum angular speed of the robot. + * + * @param max_angular_speed The maximum angular speed of the robot. + */ + void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; + + void setWheelRadius(const float wheel_radius) {_r = wheel_radius;}; + + void setArmed(const bool armed) { _armed = armed; }; + + void allocate(); + + /** + * @brief Computes the inverse kinematics for mecanum drive. + * + * @param linear_velocity_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::Vector4f computeInverseKinematics(float linear_velocity_x, float linear_velocity_y, float yaw_rate) const; + +private: + uORB::Subscription _mecanum_drive_control_output_sub{ORB_ID(mecanum_drive_control_output)}; + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + + mecanum_drive_setpoint_s _mecanum_drive_control_output{}; + bool _armed = false; + + float _lx{0.f}; + float _ly{0.f}; + float _vx_max{0.f}; + float _vy_max{0.f}; + float _r{1.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematicsTest.cpp b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematicsTest.cpp new file mode 100644 index 0000000000..220ea67f57 --- /dev/null +++ b/src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematicsTest.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (C) 2023-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 +#include "MecanumDriveKinematics.hpp" +#include + +using namespace matrix; + +class MecanumDriveKinematicsTest : public ::testing::Test +{ +public: + MecanumDriveKinematics kinematics{nullptr}; +}; + +TEST_F(MecanumDriveKinematicsTest, AllZeroInputCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with zero linear velocity and zero yaw rate (stationary vehicle) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); +} + + +TEST_F(MecanumDriveKinematicsTest, InvalidParameterCase) +{ + kinematics.setWheelBase(0.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with invalid parameters (zero wheel base and wheel radius) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); +} + + +TEST_F(MecanumDriveKinematicsTest, UnitCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(10.f); + kinematics.setMaxAngularVelocity(10.f); + + // Test with unit values for linear velocity and yaw rate + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); +} + + +TEST_F(MecanumDriveKinematicsTest, UnitSaturationCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); +} + + +TEST_F(MecanumDriveKinematicsTest, OppositeUnitSaturationCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); +} + +TEST_F(MecanumDriveKinematicsTest, RandomCase) +{ + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Negative linear velocity for backward motion and positive yaw rate for turning right + EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); +} + +TEST_F(MecanumDriveKinematicsTest, RotateInPlaceCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test rotating in place (zero linear velocity, non-zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); +} + +TEST_F(MecanumDriveKinematicsTest, StraightMovementCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test moving straight (non-zero linear velocity, zero yaw rate) + EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST_F(MecanumDriveKinematicsTest, MinInputValuesCase) +{ + kinematics.setWheelBase(FLT_MIN); + kinematics.setMaxSpeed(FLT_MIN); + kinematics.setMaxAngularVelocity(FLT_MIN); + + // Test with minimum possible input values + EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); +} + +TEST_F(MecanumDriveKinematicsTest, MaxSpeedLimitCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); +} + +TEST_F(MecanumDriveKinematicsTest, MaxSpeedForwardsCase) +{ + kinematics.setWheelBase(1.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); +} + +TEST_F(MecanumDriveKinematicsTest, MaxAngularCase) +{ + kinematics.setWheelBase(2.f); + kinematics.setMaxSpeed(1.f); + kinematics.setMaxAngularVelocity(1.f); + + // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed + EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); +} diff --git a/src/modules/mecanum_drive/module.yaml b/src/modules/mecanum_drive/module.yaml new file mode 100644 index 0000000000..68f7bf37f9 --- /dev/null +++ b/src/modules/mecanum_drive/module.yaml @@ -0,0 +1,122 @@ +module_name: Mecanum Drive + +parameters: + - group: Rover Mecanum Drive + definitions: + MD_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the center of the right wheel to the center of the left wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + MD_WHEEL_RADIUS: + description: + short: Wheel radius + long: Size of the wheel, half the diameter of the wheel + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.1 + MD_SPEED_SCALE: + description: + short: Manual speed scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + MD_ANG_SCALE: + description: + short: Manual angular velocity scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + MD_WHEEL_SPEED: + description: + short: Maximum wheel speed + type: float + unit: rad/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.3 + MD_P_HEADING: + description: + short: Proportional gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + MD_P_SPEED: + description: + short: Proportional gain for speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + MD_I_SPEED: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + MD_P_ANG_VEL: + description: + short: Proportional gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + MD_I_ANG_VEL: + description: + short: Integral gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + MD_MAX_JERK: + description: + short: Maximum jerk + long: Limit for forwards acc/deceleration change. + type: float + unit: m/s^3 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + MD_MAX_ACCEL: + description: + short: Maximum acceleration + long: Maximum acceleration is used to limit the acceleration of the rover + type: float + unit: m/s^2 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 2fed14dfe7..908b47d124 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -63,6 +63,8 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA { unsigned active_output_count = 0; + // printf("num_outputs: %d\n", num_outputs); + for (unsigned i = 0; i < num_outputs; i++) { if (_mixing_output.isFunctionSet(i)) { active_output_count++; @@ -72,6 +74,10 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA } } + // printf("active_output_count: %d\n", active_output_count); + + // active_output_count = 4; + if (active_output_count > 0) { gz::msgs::Actuators wheel_velocity_message; wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0); @@ -79,10 +85,14 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA for (unsigned i = 0; i < active_output_count; i++) { // Offsetting the output allows for negative values despite unsigned integer to reverse the wheels static constexpr float output_offset = 100.0f; + // printf("outputs[%d]: %d\n", i, outputs[i]); float scaled_output = (float)outputs[i] - output_offset; + // printf("scaled_output: %f\n", (double)scaled_output); wheel_velocity_message.set_velocity(i, scaled_output); } + // printf("\n"); + if (_actuators_pub.Valid()) { return _actuators_pub.Publish(wheel_velocity_message); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index 281606dd61..89f9495152 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -50,6 +50,7 @@ class GZMixingInterfaceWheel : public OutputModuleInterface { public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + // static constexpr int MAX_ACTUATORS = 4; GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) : OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl), diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 35cccb61bc..1b38e13d98 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -33,4 +33,4 @@ actuator_output: min: { min: 0, max: 200, default: 0 } max: { min: 0, max: 200, default: 200 } failsafe: { min: 0, max: 200 } - num_channels: 2 + num_channels: 4