fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 19:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Differential Rover: PR fixes

Browse Source
This commit is contained in:
Matthias Grob
2023-12-21 16:27:53 +01:00
committed by Per Frivik
parent 056e41af8c
commit 3de5c609a4
35 changed files with 381 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/differential_drive_control/DifferentialDriveControl.cpp
+75 -93
View File
@@ -34,7 +34,7 @@
#include "DifferentialDriveControl.hpp"
using namespace time_literals;
using namespace matrix;
namespace differential_drive_control
{
@@ -42,136 +42,118 @@ DifferentialDriveControl::DifferentialDriveControl() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
_outputs_pub.advertise();
_last_timestamp = hrt_absolute_time();
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
updateParams();
}
int DifferentialDriveControl::task_spawn(int argc, char *argv[])
{
DifferentialDriveControl *obj = new DifferentialDriveControl();
if (!obj) {
PX4_ERR("alloc failed");
return -1;
}
_object.store(obj);
_task_id = task_id_is_work_queue;
obj->start();
return 0;
}
void DifferentialDriveControl::start()
bool DifferentialDriveControl::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
void DifferentialDriveControl::updateParams()
{
ModuleParams::updateParams();
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
_differential_drive_kinematics.setMaxSpeed(_max_speed);
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
}
void DifferentialDriveControl::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
}
_current_timestamp = hrt_absolute_time();
vehicle_control_mode_poll();
hrt_abstime now = hrt_absolute_time();
if (_parameter_update_sub.updated()) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParams();
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
}
if (_vehicle_control_mode.flag_armed) {
if (_vehicle_control_mode_sub.updated()) {
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_armed = vehicle_control_mode.flag_armed;
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
}
}
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{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
// Manual mode
if (_vehicle_control_mode.flag_control_manual_enabled) {
// directly get the input from the manual control setpoint (joystick)
_differential_drive_setpoint.timestamp = hrt_absolute_time();
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_max_speed.get();
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_max_angular_velocity.get();
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
_differential_drive_setpoint.timestamp = now;
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
_max_angular_velocity;
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
}
}
}
// if manual is not enabled, it will look for a differential drive setpoint topic (differential_drive_setpoint) and subscribe to it from a ROS2 mode over DDS
if (_differential_drive_setpoint_sub.updated()) {
_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
_velocity_control_inputs(0) = _differential_drive_setpoint.speed;
_velocity_control_inputs(1) = _differential_drive_setpoint.yaw_rate;
// publish data to actuator_motors (output module)
// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
_differential_drive_setpoint.speed,
_differential_drive_setpoint.yaw_rate);
} else if (_differential_drive_setpoint.timestamp <= 100_ms) {
_velocity_control_inputs(0) = 0.0f;
_velocity_control_inputs(1) = 0.0f;
// Check if max_angular_wheel_speed is zero
const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
if (!_armed || setpoint_timeout || !valid_max_speed) {
wheel_speeds = {}; // stop
}
wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
wheel_speeds.copyTo(actuator_motors.control);
actuator_motors.timestamp = now;
_actuator_motors_pub.publish(actuator_motors);
}
int DifferentialDriveControl::task_spawn(int argc, char *argv[])
{
DifferentialDriveControl *instance = new DifferentialDriveControl();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
// if the system is in an error state, stop the vehicle
_velocity_control_inputs = {0.0f, 0.0f};
PX4_ERR("alloc failed");
}
// publish data to actuator_motors (output module)
publishWheelControl();
_last_timestamp = _current_timestamp;
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
void DifferentialDriveControl::publishWheelControl()
int DifferentialDriveControl::custom_command(int argc, char *argv[])
{
// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
_output_inverse = _differential_drive_kinematics.computeInverseKinematics(_velocity_control_inputs(0),
_velocity_control_inputs(1));
// Superpose Linear and Angular velocity vector
float max_angular_wheel_speed = ((_param_rdd_max_speed.get() + (_param_rdd_max_angular_velocity.get() *
_param_rdd_wheel_base.get() / 2.f)) / _param_rdd_wheel_radius.get());
// Check if max_angular_wheel_speed is zero
if (fabsf(max_angular_wheel_speed) < FLT_EPSILON) {
// Guard against division by zero
max_angular_wheel_speed = 0.001f;
}
_actuator_motors.timestamp = hrt_absolute_time();
// bitset which motors are configured to be reversible (3 -> ..00 0000 0011 this means that the first two motors are reversible)
_actuator_motors.reversible_flags = 3;
_actuator_motors.control[0] = _output_inverse(0) / max_angular_wheel_speed;
_actuator_motors.control[1] = _output_inverse(1) / max_angular_wheel_speed;
_outputs_pub.publish(_actuator_motors);
}
void DifferentialDriveControl::vehicle_control_mode_poll()
{
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
}
}
int DifferentialDriveControl::print_status()
{
PX4_INFO("Diffential Drive Controller running");
return 0;
return print_usage("unknown command");
}
int DifferentialDriveControl::print_usage(const char *reason)
@@ -186,8 +168,8 @@ int DifferentialDriveControl::print_usage(const char *reason)
Rover Differential Drive controller.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("differential_drive", "system");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}