forked from Archive/PX4-Autopilot
DifferentialDrive: Rework structure
3 Components Guidance - Control - Allocation with their corresponding uORB interface.
This commit is contained in:
parent
3b2659915a
commit
cec7535de9
|
@ -1,6 +1,8 @@
|
||||||
uint64 timestamp # time since system start (microseconds)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
float32 speed # [m/s] collective roll-off speed in body x-axis
|
float32 speed # [m/s] collective roll-off speed in body x-axis
|
||||||
|
bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
|
||||||
float32 yaw_rate # [rad/s] yaw rate
|
float32 yaw_rate # [rad/s] yaw rate
|
||||||
|
bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
|
||||||
|
|
||||||
# TOPICS differential_drive_setpoint differential_drive_control_output
|
# TOPICS differential_drive_setpoint differential_drive_control_output
|
||||||
|
|
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -31,8 +31,9 @@
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
add_subdirectory(DifferentialDriveKinematics)
|
add_subdirectory(DifferentialDriveControl)
|
||||||
add_subdirectory(DifferentialDriveGuidance)
|
add_subdirectory(DifferentialDriveGuidance)
|
||||||
|
add_subdirectory(DifferentialDriveKinematics)
|
||||||
|
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE modules__differential_drive
|
MODULE modules__differential_drive
|
||||||
|
@ -41,9 +42,9 @@ px4_add_module(
|
||||||
DifferentialDrive.cpp
|
DifferentialDrive.cpp
|
||||||
DifferentialDrive.hpp
|
DifferentialDrive.hpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
DifferentialDriveKinematics
|
DifferentialDriveControl
|
||||||
DifferentialDriveGuidance
|
DifferentialDriveGuidance
|
||||||
pid
|
DifferentialDriveKinematics
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
modules__control_allocator # for parameter CA_R_REV
|
modules__control_allocator # for parameter CA_R_REV
|
||||||
MODULE_CONFIG
|
MODULE_CONFIG
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -33,16 +33,11 @@
|
||||||
|
|
||||||
#include "DifferentialDrive.hpp"
|
#include "DifferentialDrive.hpp"
|
||||||
|
|
||||||
using namespace time_literals;
|
|
||||||
using namespace matrix;
|
|
||||||
|
|
||||||
DifferentialDrive::DifferentialDrive() :
|
DifferentialDrive::DifferentialDrive() :
|
||||||
ModuleParams(nullptr),
|
ModuleParams(nullptr),
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||||
{
|
{
|
||||||
updateParams();
|
updateParams();
|
||||||
pid_init(&_angular_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
|
|
||||||
pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool DifferentialDrive::init()
|
bool DifferentialDrive::init()
|
||||||
|
@ -55,28 +50,15 @@ void DifferentialDrive::updateParams()
|
||||||
{
|
{
|
||||||
ModuleParams::updateParams();
|
ModuleParams::updateParams();
|
||||||
|
|
||||||
pid_set_parameters(&_angular_velocity_pid,
|
|
||||||
_param_rdd_p_gain_angular_velocity.get(), // Proportional gain
|
|
||||||
_param_rdd_i_gain_angular_velocity.get(), // Integral gain
|
|
||||||
0, // Derivative gain
|
|
||||||
20, // Integral limit
|
|
||||||
200); // Output limit
|
|
||||||
|
|
||||||
pid_set_parameters(&_speed_pid,
|
|
||||||
_param_rdd_p_gain_speed.get(), // Proportional gain
|
|
||||||
_param_rdd_i_gain_speed.get(), // Integral gain
|
|
||||||
0, // Derivative gain
|
|
||||||
2, // Integral limit
|
|
||||||
200); // Output limit
|
|
||||||
|
|
||||||
_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.setWheelBase(_param_rdd_wheel_base.get());
|
||||||
|
|
||||||
|
_max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get();
|
||||||
|
_differential_drive_guidance.setMaxSpeed(_max_speed);
|
||||||
_differential_drive_kinematics.setMaxSpeed(_max_speed);
|
_differential_drive_kinematics.setMaxSpeed(_max_speed);
|
||||||
_differential_guidance_controller.setMaxSpeed(_max_speed);
|
|
||||||
|
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
|
||||||
|
_differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
|
||||||
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
|
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
|
||||||
_differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void DifferentialDrive::Run()
|
void DifferentialDrive::Run()
|
||||||
|
@ -87,13 +69,12 @@ void DifferentialDrive::Run()
|
||||||
}
|
}
|
||||||
|
|
||||||
hrt_abstime now = hrt_absolute_time();
|
hrt_abstime now = hrt_absolute_time();
|
||||||
const double dt = static_cast<double>(math::min((now - _time_stamp_last), kTimeoutUs)) / 1e6;
|
const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
|
||||||
_time_stamp_last = now;
|
_time_stamp_last = now;
|
||||||
|
|
||||||
if (_parameter_update_sub.updated()) {
|
if (_parameter_update_sub.updated()) {
|
||||||
parameter_update_s parameter_update;
|
parameter_update_s parameter_update;
|
||||||
_parameter_update_sub.copy(¶meter_update);
|
_parameter_update_sub.copy(¶meter_update);
|
||||||
|
|
||||||
updateParams();
|
updateParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -101,34 +82,12 @@ void DifferentialDrive::Run()
|
||||||
vehicle_control_mode_s vehicle_control_mode{};
|
vehicle_control_mode_s vehicle_control_mode{};
|
||||||
|
|
||||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||||
_armed = vehicle_control_mode.flag_armed;
|
_differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed);
|
||||||
_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
|
_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
|
||||||
_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
|
_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_vehicle_attitude_sub.updated()) {
|
|
||||||
vehicle_attitude_s vehicle_attitude{};
|
|
||||||
|
|
||||||
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
|
|
||||||
_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
|
|
||||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_vehicle_angular_velocity_sub.updated()) {
|
|
||||||
_vehicle_angular_velocity_sub.copy(&_vehicle_angular_velocity);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_vehicle_local_position_sub.updated()) {
|
|
||||||
vehicle_local_position_s vehicle_local_position{};
|
|
||||||
|
|
||||||
if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
|
|
||||||
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
|
||||||
_velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_manual_driving) {
|
if (_manual_driving) {
|
||||||
// Manual mode
|
// Manual mode
|
||||||
// directly produce setpoints from the manual control setpoint (joystick)
|
// directly produce setpoints from the manual control setpoint (joystick)
|
||||||
|
@ -136,66 +95,26 @@ void DifferentialDrive::Run()
|
||||||
manual_control_setpoint_s manual_control_setpoint{};
|
manual_control_setpoint_s manual_control_setpoint{};
|
||||||
|
|
||||||
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
|
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
|
||||||
_differential_drive_setpoint.timestamp = now;
|
differential_drive_setpoint_s setpoint{};
|
||||||
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
|
setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
|
||||||
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
|
setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
|
||||||
_max_angular_velocity;
|
setpoint.timestamp = now;
|
||||||
_differential_drive_control_output_pub.publish(_differential_drive_setpoint);
|
_differential_drive_setpoint_pub.publish(setpoint);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_mission_driving) {
|
} else if (_mission_driving) {
|
||||||
// Mission mode
|
// Mission mode
|
||||||
// directly receive setpoints from the guidance library
|
// directly receive setpoints from the guidance library
|
||||||
matrix::Vector2f guidance_output =
|
_differential_drive_guidance.computeGuidance(
|
||||||
_differential_guidance_controller.computeGuidance(
|
_differential_drive_control.getVehicleYaw(),
|
||||||
_vehicle_yaw,
|
_differential_drive_control.getVehicleBodyYawRate(),
|
||||||
_vehicle_angular_velocity.xyz[2],
|
|
||||||
dt
|
dt
|
||||||
);
|
);
|
||||||
|
|
||||||
_differential_drive_setpoint.timestamp = now;
|
|
||||||
_differential_drive_setpoint.speed = guidance_output(0);
|
|
||||||
_differential_drive_setpoint.yaw_rate = guidance_output(1);
|
|
||||||
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// check if the topic is updated and update the setpoint
|
_differential_drive_control.control(dt);
|
||||||
if (_differential_drive_control_output_sub.updated()) {
|
_differential_drive_kinematics.allocate();
|
||||||
_differential_drive_control_output_sub.copy(&_differential_drive_setpoint);
|
|
||||||
|
|
||||||
_speed_pid_output = 0;
|
|
||||||
_angular_velocity_pid_output = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_differential_drive_setpoint_sub.updated()) {
|
|
||||||
_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
|
|
||||||
|
|
||||||
_speed_pid_output = pid_calculate(&_speed_pid, _differential_drive_setpoint.speed, _velocity_in_body_frame(0), 0, dt);
|
|
||||||
_angular_velocity_pid_output = pid_calculate(&_angular_velocity_pid, _differential_drive_setpoint.yaw_rate,
|
|
||||||
_vehicle_angular_velocity.xyz[2], 0, dt);
|
|
||||||
}
|
|
||||||
|
|
||||||
// get the normalized wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
|
|
||||||
Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
|
|
||||||
_differential_drive_setpoint.speed + _speed_pid_output,
|
|
||||||
_differential_drive_setpoint.yaw_rate + _angular_velocity_pid_output);
|
|
||||||
|
|
||||||
// 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 DifferentialDrive::task_spawn(int argc, char *argv[])
|
int DifferentialDrive::task_spawn(int argc, char *argv[])
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -33,45 +33,24 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
// PX4 includes
|
|
||||||
#include <px4_platform_common/px4_config.h>
|
#include <px4_platform_common/px4_config.h>
|
||||||
#include <px4_platform_common/defines.h>
|
#include <px4_platform_common/defines.h>
|
||||||
#include <px4_platform_common/module.h>
|
#include <px4_platform_common/module.h>
|
||||||
#include <px4_platform_common/module_params.h>
|
#include <px4_platform_common/module_params.h>
|
||||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
|
||||||
// uORB includes
|
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
|
||||||
#include <uORB/topics/actuator_motors.h>
|
|
||||||
#include <uORB/PublicationMulti.hpp>
|
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
|
||||||
#include <uORB/topics/parameter_update.h>
|
|
||||||
#include <uORB/topics/vehicle_status.h>
|
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
|
||||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
|
||||||
#include <uORB/topics/vehicle_local_position.h>
|
|
||||||
|
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
|
||||||
#include <uORB/topics/position_setpoint_triplet.h>
|
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
|
||||||
|
|
||||||
#include <uORB/Publication.hpp>
|
#include <uORB/Publication.hpp>
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
#include <uORB/SubscriptionMultiArray.hpp>
|
|
||||||
#include <uORB/topics/differential_drive_setpoint.h>
|
#include <uORB/topics/differential_drive_setpoint.h>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
|
||||||
// Standard library includes
|
#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
|
||||||
#include <math.h>
|
#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
|
||||||
#include <lib/pid/pid.h>
|
#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"
|
||||||
|
|
||||||
// Local includes
|
|
||||||
#include <DifferentialDriveKinematics.hpp>
|
|
||||||
#include <DifferentialDriveGuidance.hpp>
|
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
||||||
static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out
|
|
||||||
|
|
||||||
class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
|
class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
|
||||||
public px4::ScheduledWorkItem
|
public px4::ScheduledWorkItem
|
||||||
{
|
{
|
||||||
|
@ -95,55 +74,28 @@ protected:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void Run() override;
|
void Run() override;
|
||||||
|
|
||||||
uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
|
|
||||||
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
|
|
||||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
|
||||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
|
||||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
|
||||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
|
||||||
|
|
||||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
|
||||||
uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
|
|
||||||
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
|
||||||
differential_drive_setpoint_s _differential_drive_setpoint{};
|
|
||||||
vehicle_angular_velocity_s _vehicle_angular_velocity{};
|
|
||||||
bool _armed = false;
|
|
||||||
bool _manual_driving = false;
|
bool _manual_driving = false;
|
||||||
bool _mission_driving = false;
|
bool _mission_driving = false;
|
||||||
|
|
||||||
hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
|
hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
|
||||||
|
|
||||||
DifferentialDriveKinematics _differential_drive_kinematics;
|
DifferentialDriveGuidance _differential_drive_guidance{this};
|
||||||
DifferentialDriveGuidance _differential_guidance_controller{this};
|
DifferentialDriveControl _differential_drive_control{this};
|
||||||
|
DifferentialDriveKinematics _differential_drive_kinematics{this};
|
||||||
|
|
||||||
float _max_speed{0.f};
|
float _max_speed{0.f};
|
||||||
float _max_angular_velocity{0.f};
|
float _max_angular_velocity{0.f};
|
||||||
|
|
||||||
matrix::Quatf _vehicle_attitude_quaternion{};
|
|
||||||
float _vehicle_yaw{0.f};
|
|
||||||
Vector3f _velocity_in_body_frame{0.f, 0.f, 0.f};
|
|
||||||
|
|
||||||
PID_t _angular_velocity_pid; ///< The PID controller for yaw rate.
|
|
||||||
PID_t _speed_pid; ///< The PID controller for velocity.
|
|
||||||
|
|
||||||
float _speed_pid_output{0.f};
|
|
||||||
float _angular_velocity_pid_output{0.f};
|
|
||||||
|
|
||||||
DEFINE_PARAMETERS(
|
DEFINE_PARAMETERS(
|
||||||
(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
|
|
||||||
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
|
|
||||||
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
|
|
||||||
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_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_ANG_SCALE>) _param_rdd_ang_velocity_scale,
|
||||||
(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
|
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
|
||||||
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
|
(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_SPEED>) _param_rdd_wheel_speed,
|
||||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
|
||||||
)
|
)
|
||||||
};
|
};
|
||||||
|
|
|
@ -0,0 +1,39 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (c) 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
|
||||||
|
# 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
px4_add_library(DifferentialDriveControl
|
||||||
|
DifferentialDriveControl.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(DifferentialDriveControl PUBLIC pid)
|
||||||
|
target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
@ -0,0 +1,109 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "DifferentialDriveControl.hpp"
|
||||||
|
|
||||||
|
using namespace matrix;
|
||||||
|
|
||||||
|
DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
|
||||||
|
{
|
||||||
|
pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||||
|
pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||||
|
}
|
||||||
|
|
||||||
|
void DifferentialDriveControl::updateParams()
|
||||||
|
{
|
||||||
|
ModuleParams::updateParams();
|
||||||
|
|
||||||
|
pid_set_parameters(&_pid_angular_velocity,
|
||||||
|
_param_rdd_p_gain_angular_velocity.get(), // Proportional gain
|
||||||
|
_param_rdd_i_gain_angular_velocity.get(), // Integral gain
|
||||||
|
0, // Derivative gain
|
||||||
|
20, // Integral limit
|
||||||
|
200); // Output limit
|
||||||
|
|
||||||
|
pid_set_parameters(&_pid_speed,
|
||||||
|
_param_rdd_p_gain_speed.get(), // Proportional gain
|
||||||
|
_param_rdd_i_gain_speed.get(), // Integral gain
|
||||||
|
0, // Derivative gain
|
||||||
|
2, // Integral limit
|
||||||
|
200); // Output limit
|
||||||
|
}
|
||||||
|
|
||||||
|
void DifferentialDriveControl::control(float dt)
|
||||||
|
{
|
||||||
|
if (_vehicle_angular_velocity_sub.updated()) {
|
||||||
|
vehicle_angular_velocity_s vehicle_angular_velocity{};
|
||||||
|
|
||||||
|
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
|
||||||
|
_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_vehicle_attitude_sub.updated()) {
|
||||||
|
vehicle_attitude_s vehicle_attitude{};
|
||||||
|
|
||||||
|
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
|
||||||
|
_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
|
||||||
|
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_vehicle_local_position_sub.updated()) {
|
||||||
|
vehicle_local_position_s vehicle_local_position{};
|
||||||
|
|
||||||
|
if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
|
||||||
|
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
||||||
|
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||||
|
_vehicle_forward_speed = velocity_in_body_frame(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
|
||||||
|
|
||||||
|
// PID to reach setpoint using control_output
|
||||||
|
differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
|
||||||
|
|
||||||
|
if (_differential_drive_setpoint.closed_loop_speed_control) {
|
||||||
|
differential_drive_control_output.speed +=
|
||||||
|
pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_differential_drive_setpoint.closed_loop_yaw_rate_control) {
|
||||||
|
differential_drive_control_output.yaw_rate +=
|
||||||
|
pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
differential_drive_control_output.timestamp = hrt_absolute_time();
|
||||||
|
_differential_drive_control_output_pub.publish(differential_drive_control_output);
|
||||||
|
}
|
|
@ -0,0 +1,91 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file DifferentialDriveControl.hpp
|
||||||
|
*
|
||||||
|
* Controller for heading rate and forward speed.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <lib/pid/pid.h>
|
||||||
|
#include <matrix/matrix/math.hpp>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/topics/differential_drive_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||||
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_local_position.h>
|
||||||
|
|
||||||
|
class DifferentialDriveControl : public ModuleParams
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DifferentialDriveControl(ModuleParams *parent);
|
||||||
|
~DifferentialDriveControl() = default;
|
||||||
|
|
||||||
|
void control(float dt);
|
||||||
|
float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
|
||||||
|
float getVehicleYaw() const { return _vehicle_yaw; }
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void updateParams() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||||
|
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||||
|
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||||
|
|
||||||
|
uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
|
||||||
|
|
||||||
|
differential_drive_setpoint_s _differential_drive_setpoint{};
|
||||||
|
|
||||||
|
matrix::Quatf _vehicle_attitude_quaternion{};
|
||||||
|
float _vehicle_yaw{0.f};
|
||||||
|
|
||||||
|
// States
|
||||||
|
float _vehicle_body_yaw_rate{0.f};
|
||||||
|
float _vehicle_forward_speed{0.f};
|
||||||
|
|
||||||
|
PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
|
||||||
|
PID_t _pid_speed; ///< The PID controller for velocity.
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
|
||||||
|
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
|
||||||
|
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
|
||||||
|
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
|
||||||
|
)
|
||||||
|
};
|
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -33,10 +33,6 @@
|
||||||
|
|
||||||
px4_add_library(DifferentialDriveGuidance
|
px4_add_library(DifferentialDriveGuidance
|
||||||
DifferentialDriveGuidance.cpp
|
DifferentialDriveGuidance.cpp
|
||||||
DifferentialDriveGuidance.hpp
|
|
||||||
)
|
)
|
||||||
|
|
||||||
target_compile_options(DifferentialDriveGuidance PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
|
||||||
target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
# px4_add_unit_gtest(SRC DifferentialDriveGuidanceTest.cpp LINKLIBS DifferentialDriveGuidance)
|
|
||||||
|
|
|
@ -1,3 +1,36 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
#include "DifferentialDriveGuidance.hpp"
|
#include "DifferentialDriveGuidance.hpp"
|
||||||
|
|
||||||
#include <mathlib/math/Limits.hpp>
|
#include <mathlib/math/Limits.hpp>
|
||||||
|
@ -11,7 +44,7 @@ DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : Mod
|
||||||
pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
|
pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
|
||||||
}
|
}
|
||||||
|
|
||||||
matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, float angular_velocity, float dt)
|
void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
|
||||||
{
|
{
|
||||||
if (_position_setpoint_triplet_sub.updated()) {
|
if (_position_setpoint_triplet_sub.updated()) {
|
||||||
_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
|
_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
|
||||||
|
@ -31,7 +64,7 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
|
||||||
|
|
||||||
float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
|
float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
|
||||||
current_waypoint(1));
|
current_waypoint(1));
|
||||||
float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
|
float heading_error = matrix::wrap_pi(desired_heading - yaw);
|
||||||
|
|
||||||
const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
|
const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
|
||||||
_param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
|
_param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
|
||||||
|
@ -54,7 +87,6 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
|
||||||
_currentState = GuidanceState::GOAL_REACHED;
|
_currentState = GuidanceState::GOAL_REACHED;
|
||||||
}
|
}
|
||||||
|
|
||||||
matrix::Vector2f output;
|
|
||||||
float desired_speed = 0.f;
|
float desired_speed = 0.f;
|
||||||
|
|
||||||
switch (_currentState) {
|
switch (_currentState) {
|
||||||
|
@ -79,10 +111,12 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
|
||||||
// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
|
// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
|
||||||
float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, dt);
|
float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, dt);
|
||||||
|
|
||||||
output(0) = math::constrain(desired_speed, -_max_speed, _max_speed);
|
differential_drive_setpoint_s output{};
|
||||||
output(1) = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
|
output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
|
||||||
|
output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
|
||||||
return output;
|
output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
|
||||||
|
output.timestamp = hrt_absolute_time();
|
||||||
|
_differential_drive_setpoint_pub.publish(output);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DifferentialDriveGuidance::updateParams()
|
void DifferentialDriveGuidance::updateParams()
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -33,7 +33,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <px4_platform_common/module.h>
|
|
||||||
#include <px4_platform_common/module_params.h>
|
#include <px4_platform_common/module_params.h>
|
||||||
|
|
||||||
#include <matrix/matrix/math.hpp>
|
#include <matrix/matrix/math.hpp>
|
||||||
|
@ -48,6 +47,7 @@
|
||||||
|
|
||||||
#include <uORB/Publication.hpp>
|
#include <uORB/Publication.hpp>
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/topics/differential_drive_setpoint.h>
|
||||||
#include <uORB/topics/position_setpoint_triplet.h>
|
#include <uORB/topics/position_setpoint_triplet.h>
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
|
||||||
|
@ -74,10 +74,6 @@ public:
|
||||||
* @param parent The parent ModuleParams object.
|
* @param parent The parent ModuleParams object.
|
||||||
*/
|
*/
|
||||||
DifferentialDriveGuidance(ModuleParams *parent);
|
DifferentialDriveGuidance(ModuleParams *parent);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Default destructor.
|
|
||||||
*/
|
|
||||||
~DifferentialDriveGuidance() = default;
|
~DifferentialDriveGuidance() = default;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -89,10 +85,8 @@ public:
|
||||||
* @param body_velocity The velocity of the vehicle in m/s.
|
* @param body_velocity The velocity of the vehicle in m/s.
|
||||||
* @param angular_velocity The angular velocity of the vehicle in rad/s.
|
* @param angular_velocity The angular velocity of the vehicle in rad/s.
|
||||||
* @param dt The time step in seconds.
|
* @param dt The time step in seconds.
|
||||||
* @return A 2D vector containing the computed guidance (speed in m/s, yaw rate in rad/s).
|
|
||||||
*/
|
*/
|
||||||
matrix::Vector2f computeGuidance(float vehicle_yaw,
|
void computeGuidance(float yaw, float angular_velocity, float dt);
|
||||||
float angular_velocity, float dt);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Set the maximum speed for the vehicle.
|
* @brief Set the maximum speed for the vehicle.
|
||||||
|
@ -116,10 +110,11 @@ protected:
|
||||||
void updateParams() override;
|
void updateParams() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||||
|
|
||||||
|
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
|
||||||
position_setpoint_triplet_s _position_setpoint_triplet{};
|
position_setpoint_triplet_s _position_setpoint_triplet{};
|
||||||
vehicle_global_position_s _vehicle_global_position{};
|
vehicle_global_position_s _vehicle_global_position{};
|
||||||
|
|
||||||
|
@ -143,5 +138,4 @@ private:
|
||||||
(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
|
(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
|
||||||
(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
|
(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
|
||||||
)
|
)
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# 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})
|
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)
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -36,8 +36,38 @@
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
using namespace matrix;
|
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) {
|
if (_max_speed < FLT_EPSILON) {
|
||||||
return Vector2f();
|
return Vector2f();
|
||||||
|
@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
|
||||||
|
|
||||||
if (combined_velocity > _max_speed) {
|
if (combined_velocity > _max_speed) {
|
||||||
float excess_velocity = fabsf(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);
|
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -34,6 +34,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <matrix/matrix/math.hpp>
|
#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.
|
* @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
|
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
|
||||||
* given linear velocity and yaw rate.
|
* given linear velocity and yaw rate.
|
||||||
*/
|
*/
|
||||||
class DifferentialDriveKinematics
|
class DifferentialDriveKinematics : public ModuleParams
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
DifferentialDriveKinematics() = default;
|
DifferentialDriveKinematics(ModuleParams *parent);
|
||||||
~DifferentialDriveKinematics() = default;
|
~DifferentialDriveKinematics() = default;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -68,6 +73,10 @@ public:
|
||||||
*/
|
*/
|
||||||
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
|
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.
|
* @brief Computes the inverse kinematics for differential drive.
|
||||||
*
|
*
|
||||||
|
@ -75,10 +84,20 @@ public:
|
||||||
* @param yaw_rate Yaw rate of the robot.
|
* @param yaw_rate Yaw rate of the robot.
|
||||||
* @return matrix::Vector2f Motor velocities for the right and left motors.
|
* @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:
|
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 _wheel_base{0.f};
|
||||||
float _max_speed{0.f};
|
float _max_speed{0.f};
|
||||||
float _max_angular_velocity{0.f};
|
float _max_angular_velocity{0.f};
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||||
|
)
|
||||||
};
|
};
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -37,9 +37,14 @@
|
||||||
|
|
||||||
using namespace matrix;
|
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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(10.f);
|
kinematics.setMaxSpeed(10.f);
|
||||||
kinematics.setMaxAngularVelocity(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.setWheelBase(0.f);
|
||||||
kinematics.setMaxSpeed(10.f);
|
kinematics.setMaxSpeed(10.f);
|
||||||
kinematics.setMaxAngularVelocity(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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(10.f);
|
kinematics.setMaxSpeed(10.f);
|
||||||
kinematics.setMaxAngularVelocity(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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
|
||||||
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
|
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.setWheelBase(2.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(FLT_MIN);
|
||||||
kinematics.setMaxSpeed(FLT_MIN);
|
kinematics.setMaxSpeed(FLT_MIN);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(1.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(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));
|
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.setWheelBase(2.f);
|
||||||
kinematics.setMaxSpeed(1.f);
|
kinematics.setMaxSpeed(1.f);
|
||||||
kinematics.setMaxAngularVelocity(1.f);
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
|
@ -43,7 +43,7 @@ parameters:
|
||||||
increment: 0.01
|
increment: 0.01
|
||||||
decimal: 2
|
decimal: 2
|
||||||
default: 1
|
default: 1
|
||||||
RDD_WHL_SPEED:
|
RDD_WHEEL_SPEED:
|
||||||
description:
|
description:
|
||||||
short: Maximum wheel speed
|
short: Maximum wheel speed
|
||||||
type: float
|
type: float
|
||||||
|
|
|
@ -127,9 +127,6 @@ subscriptions:
|
||||||
- topic: /fmu/in/vehicle_rates_setpoint
|
- topic: /fmu/in/vehicle_rates_setpoint
|
||||||
type: px4_msgs::msg::VehicleRatesSetpoint
|
type: px4_msgs::msg::VehicleRatesSetpoint
|
||||||
|
|
||||||
- topic: /fmu/in/differential_drive_control_output
|
|
||||||
type: px4_msgs::msg::DifferentialDriveSetpoint
|
|
||||||
|
|
||||||
- topic: /fmu/in/differential_drive_setpoint
|
- topic: /fmu/in/differential_drive_setpoint
|
||||||
type: px4_msgs::msg::DifferentialDriveSetpoint
|
type: px4_msgs::msg::DifferentialDriveSetpoint
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue