forked from Archive/PX4-Autopilot
Rename module differential_drive_control -> differential_drive
This commit is contained in:
parent
fc90e235f1
commit
b54b4f7dce
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
|
@ -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
|
|
|
@ -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
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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---
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue