From bb0dfba4e650d8e1dc1a3af8a3df1ef4c3212982 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Fri, 26 Jan 2024 10:04:59 +0100 Subject: [PATCH] added acro mode Acro mode is manual mode, but with rate control --- src/modules/differential_drive/DifferentialDrive.cpp | 12 ++++++++++++ src/modules/differential_drive/DifferentialDrive.hpp | 1 + 2 files changed, 13 insertions(+) diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp index 99f462a782..f020fd1827 100644 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ b/src/modules/differential_drive/DifferentialDrive.cpp @@ -84,6 +84,7 @@ void DifferentialDrive::Run() if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { _differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed); _manual_driving = vehicle_control_mode.flag_control_manual_enabled; + _acro_driving = vehicle_control_mode.flag_control_rates_enabled; _mission_driving = vehicle_control_mode.flag_control_auto_enabled; } } @@ -98,6 +99,17 @@ void DifferentialDrive::Run() differential_drive_setpoint_s setpoint{}; setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; + + // 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; _differential_drive_setpoint_pub.publish(setpoint); } diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/differential_drive/DifferentialDrive.hpp index 6c5805596e..8e1d0936cd 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/differential_drive/DifferentialDrive.hpp @@ -82,6 +82,7 @@ private: 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 */ DifferentialDriveGuidance _differential_drive_guidance{this};