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

DifferentialDrive: Rework structure

Browse Source 
3 Components Guidance - Control - Allocation
with their corresponding uORB interface.
This commit is contained in:
Matthias Grob
2024-02-12 14:29:10 +01:00
parent f85144ca76
commit 396ef222ee
16 changed files with 405 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt
+2 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2023 PX4 Development Team. All rights reserved.
# Copyright (c) 2023-2024 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
@@ -37,4 +37,4 @@ px4_add_library(DifferentialDriveKinematics
target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
+33 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
* Copyright (C) 2023-2024 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
@@ -36,8 +36,38 @@
#include <mathlib/mathlib.h>
using namespace matrix;
using namespace time_literals;
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
{}
void DifferentialDriveKinematics::allocate()
{
hrt_abstime now = hrt_absolute_time();
if (_differential_drive_control_output_sub.updated()) {
_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
}
const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now;
Vector2f wheel_speeds =
computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate);
if (!_armed || setpoint_timeout) {
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);
}
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
{
if (_max_speed < FLT_EPSILON) {
return Vector2f();
@@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
if (combined_velocity > _max_speed) {
float excess_velocity = fabsf(combined_velocity - _max_speed);
float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
}
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
+23 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2023-2024 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
@@ -34,6 +34,11 @@
#pragma once
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/differential_drive_setpoint.h>
/**
* @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
@@ -41,10 +46,10 @@
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
* given linear velocity and yaw rate.
*/
class DifferentialDriveKinematics
class DifferentialDriveKinematics : public ModuleParams
{
public:
DifferentialDriveKinematics() = default;
DifferentialDriveKinematics(ModuleParams *parent);
~DifferentialDriveKinematics() = default;
/**
@@ -68,6 +73,10 @@ public:
*/
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
void setArmed(const bool armed) { _armed = armed; };
void allocate();
/**
* @brief Computes the inverse kinematics for differential drive.
*
@@ -75,10 +84,20 @@ public:
* @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);
matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;
private:
uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
differential_drive_setpoint_s _differential_drive_control_output{};
bool _armed = false;
float _wheel_base{0.f};
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
+19 -25
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
* Copyright (C) 2023-2024 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
@@ -37,9 +37,14 @@
using namespace matrix;
TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
class DifferentialDriveKinematicsTest : public ::testing::Test
{
public:
DifferentialDriveKinematics kinematics{nullptr};
};
TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
@@ -49,9 +54,8 @@ TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
}
TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(0.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
@@ -61,9 +65,8 @@ TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
}
TEST(DifferentialDriveKinematicsTest, UnitCase)
TEST_F(DifferentialDriveKinematicsTest, UnitCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(10.f);
kinematics.setMaxAngularVelocity(10.f);
@@ -73,9 +76,8 @@ TEST(DifferentialDriveKinematicsTest, UnitCase)
}
TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -85,9 +87,8 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
}
TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
}
TEST(DifferentialDriveKinematicsTest, RandomCase)
TEST_F(DifferentialDriveKinematicsTest, RandomCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -107,9 +107,8 @@ TEST(DifferentialDriveKinematicsTest, RandomCase)
EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
}
TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -118,9 +117,8 @@ TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
}
TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -129,9 +127,8 @@ TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(FLT_MIN);
kinematics.setMaxSpeed(FLT_MIN);
kinematics.setMaxAngularVelocity(FLT_MIN);
@@ -140,9 +137,8 @@ TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
}
TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -151,9 +147,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(1.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);
@@ -162,9 +157,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
}
TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase)
{
DifferentialDriveKinematics kinematics;
kinematics.setWheelBase(2.f);
kinematics.setMaxSpeed(1.f);
kinematics.setMaxAngularVelocity(1.f);