dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Differential Rover: PR fixes

This commit is contained in:
Matthias Grob 2023-12-04 11:21:15 +01:00 committed by Per Frivik
parent 056e41af8c
commit 3de5c609a4
35 changed files with 381 additions and 478 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover
View File

@ -3,7 +3,7 @@
# @name Rover
#
. ${R}etc/init.d/rc.rover_ackermann_defaults
. ${R}etc/init.d/rc.rover_defaults
param set-default GND_L1_DIST 5
param set-default GND_L1_PERIOD 10

22
ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover
View File

@ -6,29 +6,7 @@
. ${R}etc/init.d/rc.rover_differential_defaults
param set-default GND_L1_DIST 5
param set-default GND_SP_CTRL_MODE 1
param set-default GND_SPEED_D 3
param set-default GND_SPEED_I 0.001
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_P 0.25
param set-default GND_SPEED_THR_SC 1
param set-default GND_SPEED_TRIM 4
param set-default GND_THR_CRUISE 0.3
param set-default GND_THR_MAX 0.5
param set-default GND_THR_MIN 0
param set-default NAV_ACC_RAD 0.5
param set-default NAV_LOITER_RAD 2
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 2.0
param set-default CA_AIRFRAME 6
param set-default CA_R_REV 3
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 101
param set-default PWM_MAIN_FUNC6 102
param set-default PWM_MAIN_FUNC7 102

41
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
View File

@ -1,5 +1,4 @@
#!/bin/sh
#
# @name Aion Robotics R1 Rover
# @type Rover
# @class Rover
@ -10,39 +9,23 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
param set-default SIM_GZ_EN 1
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
param set-default COM_PREARM_MODE 2
# Actuator mapping
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
param set-default SIM_GZ_WH_MIN1 0
param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_DIS1 100
param set-default NAV_ACC_RAD 0.5
param set-default NAV_LOITER_RAD 2
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
param set-default SIM_GZ_WH_MIN2 0
param set-default SIM_GZ_WH_MAX2 200
param set-default SIM_GZ_WH_DIS2 100
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 2.0
param set-default CA_R_REV 3
param set-default SIM_GZ_EC_FUNC1 101
param set-default SIM_GZ_EC_MIN1 0
param set-default SIM_GZ_EC_MAX1 200
param set-default SIM_GZ_EC_DIS1 100
param set-default SIM_GZ_EC_FUNC2 102
param set-default SIM_GZ_EC_MIN2 0
param set-default SIM_GZ_EC_MAX2 200
param set-default SIM_GZ_EC_DIS2 100
param set-default SIM_GZ_MT_FUNC1 101
param set-default SIM_GZ_MT_MIN1 0
param set-default SIM_GZ_MT_MAX1 200
param set-default SIM_GZ_MT_DIS1 100
param set-default SIM_GZ_MT_FUNC2 102
param set-default SIM_GZ_MT_MIN2 0
param set-default SIM_GZ_MT_MAX2 200
param set-default SIM_GZ_MT_DIS2 100
param set-default SIM_GZ_WH_REV 1 # reverse right wheel

2
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -53,8 +53,6 @@ px4_add_romfs_files(
rc.rover_defaults
rc.rover_differential_apps
rc.rover_differential_defaults
rc.rover_ackermann_apps
rc.rover_ackermann_defaults
rc.uuv_apps
rc.uuv_defaults
rc.vehicle_setup

2
ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle
View File

@ -13,7 +13,7 @@
# @board bitcraze_crazyflie exclude
#
. ${R}etc/init.d/rc.rover_ackermann_defaults
. ${R}etc/init.d/rc.rover_defaults
param set-default BAT1_N_CELLS 2

39
ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover
View File

@ -13,50 +13,13 @@
. ${R}etc/init.d/rc.rover_differential_defaults
param set-default BAT1_N_CELLS 4
param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
param set-default EKF2_MAG_TYPE 1
param set-default FW_AIRSPD_MIN 0
param set-default FW_AIRSPD_TRIM 1
param set-default FW_AIRSPD_MAX 3
param set-default GND_SP_CTRL_MODE 1
param set-default GND_L1_DIST 5
param set-default GND_L1_PERIOD 3
param set-default GND_THR_CRUISE 0.7
param set-default GND_THR_MAX 0.5
# Because this is differential drive, it can make a turn with radius 0.
# This corresponds to a turn angle of pi radians.
# If a special case is made for differential-drive, this will need to change.
param set-default GND_MAX_ANG 3.142
param set-default GND_WHEEL_BASE 0.3
# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
# to support negative throttle.
param set-default GND_THR_MIN 0
param set-default GND_SPEED_P 0.25
param set-default GND_SPEED_I 3
param set-default GND_SPEED_D 0.001
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_THR_SC 1
param set-default NAV_ACC_RAD 0.5
# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
param set-default GND_MAX_ANG 3.1415
# Set geometry & output configration
param set-default CA_AIRFRAME 6
param set-default CA_R_REV 3
param set-default RBCLW_ADDRESS 128
param set-default RBCLW_FUNC1 101
param set-default RBCLW_FUNC2 102
param set-default RBCLW_REV 1
param set-default RBCLW_REV 1 # reverse right wheels

28
ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps
View File

@ -1,28 +0,0 @@
#!/bin/sh
#
# Standard apps for unmanned ground vehicles (UGV).
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
ekf2 start &
#attitude_estimator_q start
#local_position_estimator start
#
# Start Control Allocator
#
control_allocator start
#
# Start attitude controllers.
#
rover_pos_control start
#
# Start Land Detector.
#
land_detector start rover

22
ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults
View File

@ -1,22 +0,0 @@
#!/bin/sh
#
# UGV default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE rover_ackermann
# MAV_TYPE_GROUND_ROVER 10
param set-default MAV_TYPE 10
# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
param set-default CBRK_AIRSPD_CHK 162128
param set-default MIS_TAKEOFF_ALT 0.01
param set-default NAV_ACC_RAD 2
param set-default NAV_LOITER_RAD 2
# Temporary.
param set-default NAV_FW_ALT_RAD 1000

2
ROMFS/px4fmu_common/init.d/rc.rover_apps
View File

@ -9,8 +9,6 @@
# Start the attitude and position estimator.
#
ekf2 start &
#attitude_estimator_q start
#local_position_estimator start
#
# Start Control Allocator

17
ROMFS/px4fmu_common/init.d/rc.rover_differential_apps
View File

@ -1,24 +1,11 @@
#!/bin/sh
#
# Standard apps for unmanned ground vehicles (UGV).
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
# Standard apps for a differential drive rover.
#
# Start the attitude and position estimator.
#
ekf2 start &
#attitude_estimator_q start
#local_position_estimator start
#
# Start manual rover differential drive controller.
#
# Start rover differential drive controller.
differential_drive_control start
#
# Start Land Detector.
#
land_detector start rover

14
ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults
View File

@ -1,11 +1,11 @@
#!/bin/sh
#
# UGV default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
# Differential rover parameters.
set VEHICLE_TYPE rover_differential
# MAV_TYPE_GROUND_ROVER 10
param set-default MAV_TYPE 10
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 6 # Rover (Differential)
param set-default CA_R_REV 3 # Right and left motors reversible
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

27
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View File

@ -23,24 +23,6 @@ then
. ${R}etc/init.d/rc.mc_apps
fi
#
# Differential Rover setup.
#
if [ $VEHICLE_TYPE = rover_differential ]
then
# Start standard UGV apps.
. ${R}etc/init.d/rc.rover_differential_apps
fi
#
# Ackermann Rover setup.
#
if [ $VEHICLE_TYPE = rover_ackermann ]
then
# Start standard UGV apps.
. ${R}etc/init.d/rc.rover_ackermann_apps
fi
#
# UGV setup.
#
@ -50,6 +32,15 @@ then
. ${R}etc/init.d/rc.rover_apps
fi
#
# Differential Rover setup.
#
if [ $VEHICLE_TYPE = rover_differential ]
then
# Start differential drive rover apps.
. ${R}etc/init.d/rc.rover_differential_apps
fi
#
# VTOL setup.
#

2
boards/px4/fmu-v5x/default.px4board
View File

@ -7,7 +7,6 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
@ -54,6 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y

2
boards/px4/sitl/default.px4board
View File

@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_EKF2=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
CONFIG_MODULES_EVENTS=y

2
platforms/common/include/px4_platform_common/module.h
View File

@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex;
* static int custom_command(int argc, char *argv[])
* {
* // support for custom commands
* // it none are supported, just do:
* // if none are supported, just do:
* return print_usage("unrecognized command");
* }
*

2
platforms/ros2/include/px4_platform_common/module.h
View File

@ -75,7 +75,7 @@
* static int custom_command(int argc, char *argv[])
* {
* // support for custom commands
* // it none are supported, just do:
* // if none are supported, just do:
* return print_usage("unrecognized command");
* }
*

3
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -61,7 +61,6 @@ ControlAllocator::ControlAllocator() :
_actuator_servos_pub.advertise();
_actuator_servos_trim_pub.advertise();
for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@ -240,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
break;
case EffectivenessSource::ROVER_DIFFERENTIAL:
// actuator_motors message is published directly from the differential drive controller
// differential_drive_control does allocation and publishes directly to actuator_motors topic
break;
case EffectivenessSource::FIXED_WING:

5
src/modules/differential_drive_control/CMakeLists.txt
View File

@ -30,17 +30,18 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_subdirectory(DifferentialDriveKinematics)
px4_add_module(
MODULE modules__differential_drive_control
MAIN differential_drive_control
COMPILE_FLAGS
#-DDEBUG_BUILD
SRCS
DifferentialDriveControl.cpp
DifferentialDriveControl.hpp
DEPENDS
DifferentialDriveKinematics
px4_work_queue
MODULE_CONFIG
module.yaml
)

168
src/modules/differential_drive_control/DifferentialDriveControl.cpp
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;
}

51
src/modules/differential_drive_control/DifferentialDriveControl.hpp
View File

@ -51,9 +51,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
// Cruise mode
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/differential_drive_setpoint.h>
// Standard library includes
@ -72,55 +69,47 @@ public:
DifferentialDriveControl();
~DifferentialDriveControl() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
bool init();
void start();
protected:
void updateParams() override;
private:
void Run() override;
void publishWheelControl();
void vehicle_control_mode_poll();
uORB::PublicationMulti<actuator_motors_s> _outputs_pub{ORB_ID(actuator_motors)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
differential_drive_setpoint_s _differential_drive_setpoint{};
vehicle_control_mode_s _vehicle_control_mode{};
actuator_motors_s _actuator_motors{};
DifferentialDriveKinematics _differential_drive_kinematics{};
DifferentialDriveKinematics _differential_drive_kinematics;
matrix::Vector2f _velocity_control_inputs{0.f, 0.f}; // [m/s] collective roll-off speed in body x-axis, [rad/s] yaw rate
matrix::Vector2f _output_inverse{0.0f, 0.0f}; // [rad/s] Right Motor, [rad/s] Left Motor
float _last_timestamp{0.f};
float _current_timestamp{0.f};
bool _armed = false;
bool _manual_driving = false;
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_MAX_SPEED>) _param_rdd_max_speed,
(ParamFloat<px4::params::RDD_MAX_ANG_VEL>) _param_rdd_max_angular_velocity,
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

2
src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt
View File

@ -33,10 +33,8 @@
px4_add_library(DifferentialDriveKinematics
DifferentialDriveKinematics.cpp
DifferentialDriveKinematics.hpp
)
target_compile_options(DifferentialDriveKinematics PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)

44
src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
View File

@ -33,20 +33,36 @@
#include "DifferentialDriveKinematics.hpp"
void DifferentialDriveKinematics::setWheelBase(float wheel_base)
#include <mathlib/mathlib.h>
using namespace matrix;
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
{
_wheel_base = wheel_base;
if (_max_speed < FLT_EPSILON) {
return Vector2f();
}
linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
// Compute an initial gain
float gain = 1.0f;
if (combined_velocity > _max_speed) {
float excess_velocity = fabsf(combined_velocity - _max_speed);
float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
}
// Apply the gain
linear_velocity_x *= gain;
// Calculate the left and right wheel speeds
return Vector2f(linear_velocity_x - rotational_velocity,
linear_velocity_x + rotational_velocity) / _max_speed;
}
void DifferentialDriveKinematics::setWheelRadius(float wheel_radius)
{
_wheel_radius = wheel_radius;
}
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_vel_x, float yaw_rate)
{
float motor_vel_right = linear_vel_x / _wheel_radius - _wheel_base / 2.f * yaw_rate / _wheel_radius;
float motor_vel_left = linear_vel_x / _wheel_radius + _wheel_base / 2.f * yaw_rate / _wheel_radius;
return matrix::Vector2f(motor_vel_right, motor_vel_left);
}

36
src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
View File

@ -47,30 +47,38 @@ public:
DifferentialDriveKinematics() = default;
~DifferentialDriveKinematics() = default;
/**
* @brief Computes the inverse kinematics for differential drive.
*
* @param linear_vel_x Linear velocity along the x-axis.
* @param yaw_rate Yaw rate of the robot.
* @return matrix::Vector2f Motor velocities for the right and left motors.
*/
matrix::Vector2f computeInverseKinematics(float linear_vel_x, float yaw_rate);
/**
* @brief Sets the wheel base of the robot.
*
* @param wheel_base The distance between the centers of the wheels.
*/
void setWheelBase(float wheel_base);
void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
/**
* @brief Sets the radius of the wheels of the robot.
* @brief Sets the maximum speed of the robot.
*
* @param wheel_radius The radius of the wheels.
* @param max_speed The maximum speed of the robot.
*/
void setWheelRadius(float wheel_radius);
void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
/**
* @brief Sets the maximum angular speed of the robot.
*
* @param max_angular_speed The maximum angular speed of the robot.
*/
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
/**
* @brief Computes the inverse kinematics for differential drive.
*
* @param linear_velocity_x Linear velocity along the x-axis.
* @param yaw_rate Yaw rate of the robot.
* @return matrix::Vector2f Motor velocities for the right and left motors.
*/
matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
private:
float _wheel_base{0.f};
float _wheel_radius{0.f};
float _max_speed{0.f};
float _max_angular_velocity{0.f};
};

148
src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
View File

@ -37,48 +37,138 @@
using namespace matrix;
TEST(DifferentialDriveKinematicsTest, AllZeroCaseInverse)
TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setWheelRadius(1.f);
Vector2f rate_setpoint = {0.f, 0.f};
kinematics.setInput(rate_setpoint, true);
Vector2f wheel_output = kinematics.getOutput(true);
EXPECT_EQ(wheel_output, Vector2f());
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with zero linear velocity and zero yaw rate (stationary vehicle)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
}
TEST(DifferentialDriveKinematicsTest, InvalidCaseInverse)
TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(0.f);
kinematics.setWheelRadius(0.f);
Vector2f rate_setpoint = {0.f, 0.f};
kinematics.setInput(rate_setpoint, true);
Vector2f wheel_output = kinematics.getOutput(true);
EXPECT_EQ(wheel_output, Vector2f());
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with invalid parameters (zero wheel base and wheel radius)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
}
TEST(DifferentialDriveKinematicsTest, UnitCaseInverse)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setWheelRadius(1.f);
Vector2f rate_setpoint = {1.f, 1.f};
kinematics.setInput(rate_setpoint, true);
Vector2f wheel_output = kinematics.getOutput(true);
Vector2f expected_output = {1.5f, 0.5f};
EXPECT_EQ(wheel_output, expected_output);
}
TEST(DifferentialDriveKinematicsTest, UnitCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setWheelRadius(1.f);
Vector2f wheel_input = {1.f, 1.f};
kinematics.setInput(wheel_input, false);
Vector2f rate_output = kinematics.getOutput(false);
Vector2f expected_output = {1.f, 0.f};
EXPECT_EQ(rate_output, expected_output);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
// Test with unit values for linear velocity and yaw rate
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
}
TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
}
TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Negative linear velocity for backward motion and positive yaw rate for turning right
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
}
TEST(DifferentialDriveKinematicsTest, RandomCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Negative linear velocity for backward motion and positive yaw rate for turning right
EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
}
TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test rotating in place (zero linear velocity, non-zero yaw rate)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
}
TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test moving straight (non-zero linear velocity, zero yaw rate)
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(FLT_MIN);
kinematics.setMaxSpeed(FLT_MIN);
kinematics.setMaxAngularVelocity(FLT_MIN);
// Test with minimum possible input values
EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
}
TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
}

2
src/modules/differential_drive_control/Kconfig
View File

@ -2,4 +2,4 @@ menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
bool "differential_drive_control"
default y
---help---
Enable support for differential_drive_control
Enable support for control of differential drive rovers

55
src/modules/differential_drive_control/module.yaml
View File

@ -0,0 +1,55 @@
module_name: Differential Drive Control
parameters:
- group: Rover Differential Drive
definitions:
RDD_WHEEL_BASE:
description:
short: Wheel base
long: Distance from the center of the right wheel to the center of the left wheel
type: float
unit: m
min: 0.001
max: 100
increment: 0.001
decimal: 3
default: 0.5
RDD_WHEEL_RADIUS:
description:
short: Wheel radius
long: Size of the wheel, half the diameter of the wheel
type: float
unit: m
min: 0.001
max: 100
increment: 0.001
decimal: 3
default: 0.1
RDD_SPEED_SCALE:
description:
short: Manual speed scale
type: float
min: 0
max: 1.0
increment: 0.01
decimal: 2
default: 1.0
RDD_ANG_SCALE:
description:
short: Manual angular velocity scale
type: float
min: 0
max: 1.0
increment: 0.01
decimal: 2
default: 1.0
RDD_WHL_SPEED:
description:
short: Maximum wheel speed
type: float
unit: rad/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 10

84
src/modules/differential_drive_control/params.c
View File

@ -1,84 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* Wheel Base
*
* Distance from the center of the right wheel to the center of the left wheel
*
* @unit m
* @min 0.001
* @max 100
* @increment 0.001
* @decimal 3
* @group Rover Differential Drive
*/
PARAM_DEFINE_FLOAT(RDD_WHEEL_BASE, 0.5f);
/**
* Wheel radius
*
* Size of the wheel, half the diameter of the wheel
*
* @unit m
* @min 0.001
* @max 100
* @increment 0.001
* @decimal 3
* @group Rover Differential Drive
*/
PARAM_DEFINE_FLOAT(RDD_WHEEL_RADIUS, 0.1f);
/**
* Max Speed
*
* @unit m/s
* @min 0.0
* @max 100
* @increment 0.01
* @decimal 2
* @group Rover Differential Drive
*/
PARAM_DEFINE_FLOAT(RDD_MAX_SPEED, 0.5f);
/**
* Max Angular Velocity
*
* @unit rad/s
* @min 0.0
* @max 100
* @increment 0.01
* @decimal 2
* @group Rover Differential Drive
*/
PARAM_DEFINE_FLOAT(RDD_MAX_ANG_VEL, 0.3f);

2
src/modules/logger/logged_topics.cpp
View File

@ -57,7 +57,7 @@ void LoggedTopics::add_default_topics()
add_topic("commander_state");
add_topic("config_overrides");
add_topic("cpuload");
add_topic("differential_drive_setpoint", 100);
add_optional_topic("differential_drive_setpoint", 100);
add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position");
add_optional_topic("external_ins_local_position");

2
src/modules/simulation/gz_bridge/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
# Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions

10
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -226,7 +226,7 @@ int GZBridge::init()
return PX4_ERROR;
}
if (!_mixing_interface_motor.init(_model_name)) {
if (!_mixing_interface_wheel.init(_model_name)) {
PX4_ERR("failed to init motor output");
return PX4_ERROR;
}
@ -729,7 +729,7 @@ void GZBridge::Run()
_mixing_interface_esc.stop();
_mixing_interface_servo.stop();
_mixing_interface_motor.stop();
_mixing_interface_wheel.stop();
exit_and_cleanup();
return;
@ -745,7 +745,7 @@ void GZBridge::Run()
_mixing_interface_esc.updateParams();
_mixing_interface_servo.updateParams();
_mixing_interface_motor.updateParams();
_mixing_interface_wheel.updateParams();
}
ScheduleDelayed(10_ms);
@ -761,8 +761,8 @@ int GZBridge::print_status()
PX4_INFO_RAW("Servo outputs:\n");
_mixing_interface_servo.mixingOutput().printStatus();
PX4_INFO_RAW("Motor outputs:\n");
_mixing_interface_motor.mixingOutput().printStatus();
PX4_INFO_RAW("Wheel outputs:\n");
_mixing_interface_wheel.mixingOutput().printStatus();
return 0;
}

2
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -132,7 +132,7 @@ private:
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
GZMixingInterfaceWheel _mixing_interface_motor{_node, _node_mutex};
GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
px4::atomic<uint64_t> _world_time_us{0};

5
src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp
View File

@ -77,8 +77,9 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
for (unsigned i = 0; i < active_output_count; i++) {
float output_scaler = 100.0f;
float scaled_output = (float)outputs[i] - output_scaler;
// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
static constexpr float output_offset = 100.0f;
float scaled_output = (float)outputs[i] - output_offset;
wheel_velocity_message.set_velocity(i, scaled_output);
}

2
src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp
View File

@ -80,7 +80,7 @@ private:
gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;
MixingOutput _mixing_output{"SIM_GZ_MT", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
gz::transport::Node::Publisher _actuators_pub;

6
src/modules/simulation/gz_bridge/module.yaml
View File

@ -25,9 +25,9 @@ actuator_output:
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 8
- param_prefix: SIM_GZ_MT
group_label: 'Motors'
channel_label: 'Motors'
- param_prefix: SIM_GZ_WH
group_label: 'Wheels'
channel_label: 'Wheels'
standard_params:
disarmed: { min: 0, max: 200, default: 100 }
min: { min: 0, max: 200, default: 0 }

6
src/modules/uxrce_dds_client/dds_topics.yaml
View File

@ -76,9 +76,6 @@ subscriptions:
- topic: /fmu/in/register_ext_component_request
type: px4_msgs::msg::RegisterExtComponentRequest
- topic: /fmu/in/differential_drive_setpoint
type: px4_msgs::msg::DifferentialDriveSetpoint
- topic: /fmu/in/unregister_ext_component
type: px4_msgs::msg::UnregisterExtComponent
@ -130,6 +127,9 @@ subscriptions:
- topic: /fmu/in/vehicle_rates_setpoint
type: px4_msgs::msg::VehicleRatesSetpoint
- topic: /fmu/in/differential_drive_setpoint
type: px4_msgs::msg::DifferentialDriveSetpoint
- topic: /fmu/in/vehicle_visual_odometry
type: px4_msgs::msg::VehicleOdometry