From 5f1edc883d6fdc90ec86f15273566e8ffe4b4ed9 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 25 Mar 2024 11:04:06 +0100 Subject: [PATCH] cleanup subscribers and publishers --- .../ackermann_drive/AckermannDrive.hpp | 11 +++------ .../AckermannDriveControl.cpp | 1 - .../AckermannDriveControl.hpp | 23 ++----------------- .../AckermannDriveKinematics.cpp | 2 +- .../AckermannDriveKinematics.hpp | 10 ++++---- src/modules/ackermann_drive/CMakeLists.txt | 9 ++++---- 6 files changed, 17 insertions(+), 39 deletions(-) diff --git a/src/modules/ackermann_drive/AckermannDrive.hpp b/src/modules/ackermann_drive/AckermannDrive.hpp index b0759510a8..8cbedc6257 100644 --- a/src/modules/ackermann_drive/AckermannDrive.hpp +++ b/src/modules/ackermann_drive/AckermannDrive.hpp @@ -41,16 +41,12 @@ #include // uORB includes -#include -#include -#include #include #include #include #include #include #include -#include #include // Standard library includes @@ -58,7 +54,7 @@ // Local includes #include "AckermannDriveControl/AckermannDriveControl.hpp" -//#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp" +//#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp" //TO BE IMPLEMENTED #include "AckermannDriveKinematics/AckermannDriveKinematics.hpp" using namespace time_literals; @@ -96,7 +92,7 @@ private: uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; AckermannDriveControl _ackermann_drive_control{this}; - //AckermannDriveGuidance _ackermann_drive_guidance{this}; + //AckermannDriveGuidance _ackermann_drive_guidance{this}; // TO BE IMPLEMENTED AckermannDriveKinematics _ackermann_drive_kinematics{this}; bool _armed = false; @@ -109,7 +105,6 @@ private: (ParamFloat) _param_rdd_ang_velocity_scale, (ParamFloat) _param_rdd_max_wheel_speed, (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_radius, - (ParamInt) _param_r_rev + (ParamFloat) _param_rdd_wheel_radius ) }; diff --git a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp index 82d5296c7c..427b224b43 100644 --- a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp +++ b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp @@ -34,7 +34,6 @@ #include "AckermannDriveControl.hpp" using namespace time_literals; -using namespace matrix; AckermannDriveControl::AckermannDriveControl(ModuleParams *parent) : ModuleParams(parent) { diff --git a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp index 317f01cdf7..7894d7c02e 100644 --- a/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp +++ b/src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp @@ -41,24 +41,14 @@ #include // uORB includes -#include -#include -#include -#include -#include -#include -#include -#include #include #include -#include #include -// Standard library includespr + +// Standard library #include -// Local includes -// #include class AckermannDriveControl : public ModuleParams { public: @@ -78,13 +68,4 @@ private: uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; differential_drive_setpoint_s _differential_drive_setpoint{}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_speed_scale, - (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_max_wheel_speed, - (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_radius, - (ParamInt) _param_r_rev - ) }; diff --git a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp index 055257a4f6..365981931c 100644 --- a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp +++ b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp @@ -52,7 +52,7 @@ void AckermannDriveKinematics::allocate() hrt_abstime now = hrt_absolute_time(); actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults + actuator_motors.reversible_flags = _param_r_rev.get(); actuator_motors.control[0] = speed; actuator_motors.control[1] = speed; actuator_motors.timestamp = now; diff --git a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp index 4570b4f82d..c04606a132 100644 --- a/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp +++ b/src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp @@ -33,14 +33,19 @@ #pragma once -#include +//px4 includes #include + +// uORB includes #include #include #include #include #include +//standard includes +#include + /** * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. * @@ -63,9 +68,6 @@ private: differential_drive_setpoint_s _differential_drive_control_output{}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; - DEFINE_PARAMETERS( (ParamInt) _param_r_rev ) diff --git a/src/modules/ackermann_drive/CMakeLists.txt b/src/modules/ackermann_drive/CMakeLists.txt index 64e5621e0e..f5b68a39e3 100644 --- a/src/modules/ackermann_drive/CMakeLists.txt +++ b/src/modules/ackermann_drive/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -add_subdirectory(AckermannDriveKinematics) - add_subdirectory(AckermannDriveControl) +add_subdirectory(AckermannDriveKinematics) +#add_subdirectory(AckermannDriveGuidance) #TO BE IMPLEMENTED px4_add_module( MODULE modules__ackermann_drive @@ -44,7 +44,8 @@ px4_add_module( DEPENDS AckermannDriveControl AckermannDriveKinematics + #AckermannDriveGuidance #TO BE IMPLEMENTED px4_work_queue - # MODULE_CONFIG - # module.yaml + #MODULE_CONFIG + # module.yaml )