Rename module differential_drive_control -> differential_drive

This commit is contained in:
Matthias Grob 2024-01-22 16:32:54 +01:00 committed by PerFrivik
parent 9d44b58af0
commit 51dd47251f
15 changed files with 25 additions and 36 deletions

View File

@ -5,7 +5,7 @@
ekf2 start & ekf2 start &
# Start rover differential drive controller. # Start rover differential drive controller.
differential_drive_control start differential_drive start
# Start Land Detector. # Start Land Detector.
land_detector start rover land_detector start rover

View File

@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y CONFIG_MODULES_EVENTS=y

View File

@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y CONFIG_MODULES_EKF2=y
CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_VERBOSE_STATUS=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y

View File

@ -35,11 +35,11 @@ add_subdirectory(DifferentialDriveKinematics)
add_subdirectory(DifferentialDriveGuidance) add_subdirectory(DifferentialDriveGuidance)
px4_add_module( px4_add_module(
MODULE modules__differential_drive_control MODULE modules__differential_drive
MAIN differential_drive_control MAIN differential_drive
SRCS SRCS
DifferentialDriveControl.cpp DifferentialDrive.cpp
DifferentialDriveControl.hpp DifferentialDrive.hpp
DEPENDS DEPENDS
DifferentialDriveKinematics DifferentialDriveKinematics
DifferentialDriveGuidance DifferentialDriveGuidance

View File

@ -31,14 +31,12 @@
* *
****************************************************************************/ ****************************************************************************/
#include "DifferentialDriveControl.hpp" #include "DifferentialDrive.hpp"
using namespace time_literals; using namespace time_literals;
using namespace matrix; using namespace matrix;
namespace differential_drive_control
{
DifferentialDriveControl::DifferentialDriveControl() : DifferentialDrive::DifferentialDrive() :
ModuleParams(nullptr), ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{ {
@ -47,13 +45,13 @@ DifferentialDriveControl::DifferentialDriveControl() :
pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f); pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
} }
bool DifferentialDriveControl::init() bool DifferentialDrive::init()
{ {
ScheduleOnInterval(10_ms); // 100 Hz ScheduleOnInterval(10_ms); // 100 Hz
return true; return true;
} }
void DifferentialDriveControl::updateParams() void DifferentialDrive::updateParams()
{ {
ModuleParams::updateParams(); ModuleParams::updateParams();
@ -81,7 +79,7 @@ void DifferentialDriveControl::updateParams()
_differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity); _differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity);
} }
void DifferentialDriveControl::Run() void DifferentialDrive::Run()
{ {
if (should_exit()) { if (should_exit()) {
ScheduleClear(); ScheduleClear();
@ -200,9 +198,9 @@ void DifferentialDriveControl::Run()
_actuator_motors_pub.publish(actuator_motors); _actuator_motors_pub.publish(actuator_motors);
} }
int DifferentialDriveControl::task_spawn(int argc, char *argv[]) int DifferentialDrive::task_spawn(int argc, char *argv[])
{ {
DifferentialDriveControl *instance = new DifferentialDriveControl(); DifferentialDrive *instance = new DifferentialDrive();
if (instance) { if (instance) {
_object.store(instance); _object.store(instance);
@ -223,12 +221,12 @@ int DifferentialDriveControl::task_spawn(int argc, char *argv[])
return PX4_ERROR; return PX4_ERROR;
} }
int DifferentialDriveControl::custom_command(int argc, char *argv[]) int DifferentialDrive::custom_command(int argc, char *argv[])
{ {
return print_usage("unknown command"); return print_usage("unknown command");
} }
int DifferentialDriveControl::print_usage(const char *reason) int DifferentialDrive::print_usage(const char *reason)
{ {
if (reason) { if (reason) {
PX4_ERR("%s\n", reason); PX4_ERR("%s\n", reason);
@ -240,15 +238,13 @@ int DifferentialDriveControl::print_usage(const char *reason)
Rover Differential Drive controller. Rover Differential Drive controller.
)DESCR_STR"); )DESCR_STR");
PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller"); PRINT_MODULE_USAGE_NAME("differential_drive", "controller");
PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0; return 0;
} }
extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[]) extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
{ {
return DifferentialDriveControl::main(argc, argv); return DifferentialDrive::main(argc, argv);
} }
} // namespace differential_drive_control

View File

@ -72,15 +72,12 @@ using namespace time_literals;
static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out
namespace differential_drive_control class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
{
class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
public px4::ScheduledWorkItem public px4::ScheduledWorkItem
{ {
public: public:
DifferentialDriveControl(); DifferentialDrive();
~DifferentialDriveControl() override = default; ~DifferentialDrive() override = default;
/** @see ModuleBase */ /** @see ModuleBase */
static int task_spawn(int argc, char *argv[]); static int task_spawn(int argc, char *argv[]);
@ -150,5 +147,3 @@ private:
(ParamInt<px4::params::CA_R_REV>) _param_r_rev (ParamInt<px4::params::CA_R_REV>) _param_r_rev
) )
}; };
} // namespace differential_drive_control

View File

@ -65,4 +65,3 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
return Vector2f(linear_velocity_x - rotational_velocity, return Vector2f(linear_velocity_x - rotational_velocity,
linear_velocity_x + rotational_velocity) / _max_speed; linear_velocity_x + rotational_velocity) / _max_speed;
} }

View File

@ -1,5 +1,5 @@
menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL menuconfig MODULES_DIFFERENTIAL_DRIVE
bool "differential_drive_control" bool "differential_drive"
default n default n
depends on MODULES_CONTROL_ALLOCATOR depends on MODULES_CONTROL_ALLOCATOR
---help--- ---help---

View File

@ -1,4 +1,4 @@
module_name: Differential Drive Control module_name: Differential Drive
parameters: parameters:
- group: Rover Differential Drive - group: Rover Differential Drive
@ -120,4 +120,3 @@ parameters:
increment: 0.01 increment: 0.01
decimal: 2 decimal: 2
default: 0.5 default: 0.5