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

rover: seperate speed control

Browse Source
This commit is contained in:
chfriedrich98
2025-08-20 10:54:16 +02:00
committed by GitHub
parent e81c62cc36
commit a1bc09a6ad
42 changed files with 381 additions and 401 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
+2 -2
View File
@@ -181,10 +181,10 @@ set(msg_files
RoverPositionSetpoint.msg
RoverRateSetpoint.msg
RoverRateStatus.msg
RoverSpeedSetpoint.msg
RoverSpeedStatus.msg
RoverSteeringSetpoint.msg
RoverThrottleSetpoint.msg
RoverVelocitySetpoint.msg
RoverVelocityStatus.msg
Rpm.msg
RtlStatus.msg
RtlTimeEstimate.msg
msg/RoverSpeedSetpoint.msg
+5
View File
@@ -0,0 +1,5 @@
# Rover Speed Setpoint
uint64 timestamp # [us] Time since system start
float32 speed_body_x # [m/s] [@range -inf (Backwards), inf (Forwards)] [@frame Body] Speed setpoint in body x direction
float32 speed_body_y # [m/s] [@range -inf (Left), inf (Right)] [@frame Body] [@invalid NaN If not mecanum] Mecanum only: Speed setpoint in body y direction
msg/RoverVelocityStatus.msg → msg/RoverSpeedStatus.msg
View File
msg/RoverVelocitySetpoint.msg
-6
View File
@@ -1,6 +0,0 @@
# Rover Velocity Setpoint
uint64 timestamp # [us] Time since system start
float32 speed # [m/s] [@range -inf (Backwards), inf (Forwards)] Speed setpoint
float32 bearing # [rad] [@range -pi,pi] [@frame NED] [@invalid: NaN, speed is defined in body x direction] Bearing setpoint
float32 yaw # [rad] [@range -pi, pi] [@frame NED] [@invalid NaN, Defaults to vehicle yaw] Mecanum only: Yaw setpoint
src/modules/logger/logged_topics.cpp
+2 -2
View File
@@ -111,10 +111,10 @@ void LoggedTopics::add_default_topics()
add_optional_topic("rover_position_setpoint", 100);
add_optional_topic("rover_rate_setpoint", 100);
add_optional_topic("rover_rate_status", 100);
add_optional_topic("rover_speed_setpoint", 100);
add_optional_topic("rover_speed_status", 100);
add_optional_topic("rover_steering_setpoint", 100);
add_optional_topic("rover_throttle_setpoint", 100);
add_optional_topic("rover_velocity_setpoint", 100);
add_optional_topic("rover_velocity_status", 100);
add_topic("rtl_time_estimate", 1000);
add_topic("rtl_status", 2000);
add_optional_topic("sensor_airflow", 100);
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp
+5 -7
View File
@@ -42,7 +42,7 @@ AckermannManualMode::AckermannManualMode(ModuleParams *parent) : ModuleParams(pa
_rover_steering_setpoint_pub.advertise();
_rover_rate_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
}
@@ -164,12 +164,10 @@ void AckermannManualMode::position()
_pos_ctl_course_direction = Vector2f(NAN, NAN);
// Speed control
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = NAN;
rover_velocity_setpoint.yaw = NAN;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = speed_setpoint;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp
+3 -3
View File
@@ -51,7 +51,7 @@
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
/**
@@ -83,7 +83,7 @@ public:
void stab();
/**
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
* @brief Generate and publish roverSpeedSetpoint/roverRateSetpoint or roverPositionSetpoint from manualControlSetpoint.
*/
void position();
@@ -109,7 +109,7 @@ private:
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables
src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp
+10 -7
View File
@@ -38,8 +38,9 @@ using namespace time_literals;
AckermannOffboardMode::AckermannOffboardMode(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
}
void AckermannOffboardMode::updateParams()
@@ -69,11 +70,13 @@ void AckermannOffboardMode::offboardControl()
} else if (offboard_control_mode.velocity) {
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = velocity_ned.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = velocity_ned.norm();
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = hrt_absolute_time();
rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp
+4 -2
View File
@@ -43,7 +43,8 @@
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
@@ -80,6 +81,7 @@ private:
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
// uORB publications
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
};
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
+29 -11
View File
@@ -38,7 +38,8 @@ using namespace time_literals;
AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_pure_pursuit_status_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
updateParams();
}
@@ -47,6 +48,7 @@ void AckermannPosControl::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
}
@@ -82,20 +84,36 @@ void AckermannPosControl::updatePosControl()
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float course_error = fabsf(matrix::wrap_pi(bearing_setpoint - _vehicle_yaw));
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed,
_param_ro_max_thr_speed.get());
speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed);
}
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = speed_setpoint;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F);
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = 0.f;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
}
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+11 -4
View File
@@ -47,7 +47,8 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_controller_status.h>
@@ -69,7 +70,7 @@ public:
~AckermannPosControl() = default;
/**
* @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
* @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint.
*/
void updatePosControl();
@@ -99,7 +100,8 @@ private:
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
@@ -110,17 +112,22 @@ private:
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
float _min_speed{NAN};
// Class Instances
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang
)
};
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp → src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp
+24 -47
View File
@@ -31,23 +31,20 @@
*
****************************************************************************/
#include "AckermannVelControl.hpp"
#include "AckermannSpeedControl.hpp"
using namespace time_literals;
AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(parent)
AckermannSpeedControl::AckermannSpeedControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_rover_speed_status_pub.advertise();
updateParams();
}
void AckermannVelControl::updateParams()
void AckermannSpeedControl::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
// Set up PID controller
_pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
@@ -58,37 +55,20 @@ void AckermannVelControl::updateParams()
if (_param_ro_accel_limit.get() > FLT_EPSILON) {
_adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
}
}
void AckermannVelControl::updateVelControl()
void AckermannSpeedControl::updateSpeedControl()
{
updateSubscriptions();
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
float max_speed = _param_ro_speed_limit.get();
// Attitude Setpoint
if (PX4_ISFINITE(_bearing_setpoint)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw));
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, max_speed);
}
}
// Throttle Setpoint
if (PX4_ISFINITE(_speed_setpoint)) {
const float speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed);
const float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed,
@@ -96,28 +76,26 @@ void AckermannVelControl::updateVelControl()
_param_ro_max_thr_speed.get(), dt);
rover_throttle_setpoint.throttle_body_y = NAN;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
// Publish position controller status (logging only)
rover_velocity_status_s rover_velocity_status;
rover_velocity_status.timestamp = _timestamp;
rover_velocity_status.measured_speed_body_x = _vehicle_speed;
rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState();
rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_velocity_status.measured_speed_body_y = NAN;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_y_integral = NAN;
_rover_velocity_status_pub.publish(rover_velocity_status);
// Publish speed controller status (logging only)
rover_speed_status_s rover_speed_status;
rover_speed_status.timestamp = _timestamp;
rover_speed_status.measured_speed_body_x = _vehicle_speed;
rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState();
rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_speed_status.measured_speed_body_y = NAN;
rover_speed_status.adjusted_speed_body_y_setpoint = NAN;
rover_speed_status.pid_throttle_body_y_integral = NAN;
_rover_speed_status_pub.publish(rover_speed_status);
}
void AckermannVelControl::updateSubscriptions()
void AckermannSpeedControl::updateSubscriptions()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_vehicle_local_position_sub.updated()) {
@@ -129,15 +107,14 @@ void AckermannVelControl::updateSubscriptions()
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_velocity_setpoint_sub.updated()) {
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
_speed_setpoint = rover_velocity_setpoint.speed;
_bearing_setpoint = rover_velocity_setpoint.bearing;
if (_rover_speed_setpoint_sub.updated()) {
rover_speed_setpoint_s rover_speed_setpoint;
_rover_speed_setpoint_sub.copy(&rover_speed_setpoint);
_speed_setpoint = rover_speed_setpoint.speed_body_x;
}
}
bool AckermannVelControl::runSanityChecks()
bool AckermannSpeedControl::runSanityChecks()
{
bool ret = true;
@@ -147,7 +124,7 @@ bool AckermannVelControl::runSanityChecks()
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
ret = false;
events::send<float>(events::ID("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error,
events::send<float>(events::ID("ackermann_speed_control_conf_invalid_speed_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
}
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp → src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp
+15 -26
View File
@@ -48,31 +48,30 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_speed_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace matrix;
/**
* @brief Class for ackermann position control.
* @brief Class for ackermann speed control.
*/
class AckermannVelControl : public ModuleParams
class AckermannSpeedControl : public ModuleParams
{
public:
/**
* @brief Constructor for AckermannVelControl.
* @brief Constructor for AckermannSpeedControl.
* @param parent The parent ModuleParams object.
*/
AckermannVelControl(ModuleParams *parent);
~AckermannVelControl() = default;
AckermannSpeedControl(ModuleParams *parent);
~AckermannSpeedControl() = default;
/**
* @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint.
* @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint.
*/
void updateVelControl();
void updateSpeedControl();
/**
* @brief Check if the necessary parameters are set.
@@ -81,9 +80,9 @@ public:
bool runSanityChecks();
/**
* @brief Reset velocity controller.
* @brief Reset speed controller.
*/
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
protected:
/**
@@ -93,30 +92,24 @@ protected:
private:
/**
* @brief Update uORB subscriptions used in position controller.
* @brief Update uORB subscriptions used in speed controller.
*/
void updateSubscriptions();
// uORB subscriptions
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)};
// uORB publications
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_status_s> _rover_speed_status_pub{ORB_ID(rover_speed_status)};
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f}; // [rad] Yaw angle of the vehicle
float _speed_setpoint{NAN};
float _bearing_setpoint{NAN};
float _min_speed{NAN};
float _max_yaw_rate{NAN};
// Controllers
PID _pid_speed;
@@ -130,10 +123,6 @@ private:
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
)
};
src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt → src/modules/rover_ackermann/AckermannSpeedControl/CMakeLists.txt
+4 -4
View File
@@ -31,9 +31,9 @@
#
############################################################################
px4_add_library(AckermannVelControl
AckermannVelControl.cpp
px4_add_library(AckermannSpeedControl
AckermannSpeedControl.cpp
)
target_link_libraries(AckermannVelControl PUBLIC PID)
target_include_directories(AckermannVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(AckermannSpeedControl PUBLIC PID)
target_include_directories(AckermannSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/rover_ackermann/CMakeLists.txt
+2 -2
View File
@@ -34,7 +34,7 @@
add_subdirectory(AckermannActControl)
add_subdirectory(AckermannRateControl)
add_subdirectory(AckermannAttControl)
add_subdirectory(AckermannVelControl)
add_subdirectory(AckermannSpeedControl)
add_subdirectory(AckermannPosControl)
add_subdirectory(AckermannDriveModes)
@@ -48,7 +48,7 @@ px4_add_module(
AckermannActControl
AckermannRateControl
AckermannAttControl
AckermannVelControl
AckermannSpeedControl
AckermannPosControl
AckermannAutoMode
AckermannManualMode
src/modules/rover_ackermann/RoverAckermann.cpp
+3 -3
View File
@@ -139,7 +139,7 @@ void RoverAckermann::updateControllers()
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_ackermann_vel_control.updateVelControl();
_ackermann_speed_control.updateSpeedControl();
}
if (_vehicle_control_mode.flag_control_attitude_enabled) {
@@ -167,7 +167,7 @@ void RoverAckermann::runSanityChecks()
return;
}
if (_vehicle_control_mode.flag_control_velocity_enabled && !_ackermann_vel_control.runSanityChecks()) {
if (_vehicle_control_mode.flag_control_velocity_enabled && !_ackermann_speed_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
@@ -182,7 +182,7 @@ void RoverAckermann::runSanityChecks()
void RoverAckermann::reset()
{
_ackermann_vel_control.reset();
_ackermann_speed_control.reset();
_ackermann_att_control.reset();
_ackermann_rate_control.reset();
_manual_mode.reset();
src/modules/rover_ackermann/RoverAckermann.hpp
+2 -2
View File
@@ -54,7 +54,7 @@
#include "AckermannActControl/AckermannActControl.hpp"
#include "AckermannRateControl/AckermannRateControl.hpp"
#include "AckermannAttControl/AckermannAttControl.hpp"
#include "AckermannVelControl/AckermannVelControl.hpp"
#include "AckermannSpeedControl/AckermannSpeedControl.hpp"
#include "AckermannPosControl/AckermannPosControl.hpp"
#include "AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp"
#include "AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp"
@@ -124,7 +124,7 @@ private:
AckermannActControl _ackermann_act_control{this};
AckermannRateControl _ackermann_rate_control{this};
AckermannAttControl _ackermann_att_control{this};
AckermannVelControl _ackermann_vel_control{this};
AckermannSpeedControl _ackermann_speed_control{this};
AckermannPosControl _ackermann_pos_control{this};
AckermannAutoMode _auto_mode{this};
AckermannManualMode _manual_mode{this};
src/modules/rover_differential/CMakeLists.txt
+3 -3
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
# Copyright (c) 2025 PX4 DeSpeedopment Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -34,7 +34,7 @@
add_subdirectory(DifferentialActControl)
add_subdirectory(DifferentialRateControl)
add_subdirectory(DifferentialAttControl)
add_subdirectory(DifferentialVelControl)
add_subdirectory(DifferentialSpeedControl)
add_subdirectory(DifferentialPosControl)
add_subdirectory(DifferentialDriveModes)
@@ -48,7 +48,7 @@ px4_add_module(
DifferentialActControl
DifferentialRateControl
DifferentialAttControl
DifferentialVelControl
DifferentialSpeedControl
DifferentialPosControl
DifferentialAutoMode
DifferentialManualMode
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp
+5 -7
View File
@@ -42,7 +42,7 @@ DifferentialManualMode::DifferentialManualMode(ModuleParams *parent) : ModulePar
_rover_steering_setpoint_pub.advertise();
_rover_rate_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
}
@@ -156,12 +156,10 @@ void DifferentialManualMode::position()
_pos_ctl_course_direction = Vector2f(NAN, NAN);
// Speed control
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = NAN;
rover_velocity_setpoint.yaw = NAN;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = speed_setpoint;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
using namespace matrix;
@@ -110,7 +110,7 @@ private:
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables
src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp
+9 -6
View File
@@ -40,7 +40,7 @@ DifferentialOffboardMode::DifferentialOffboardMode(ModuleParams *parent) : Modul
updateParams();
_rover_rate_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
}
@@ -71,11 +71,14 @@ void DifferentialOffboardMode::offboardControl()
} else if (offboard_control_mode.velocity) {
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = velocity_ned.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = velocity_ned.norm();
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = hrt_absolute_time();
rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else if (offboard_control_mode.attitude) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp
+2 -2
View File
@@ -45,7 +45,7 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
@@ -84,6 +84,6 @@ private:
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
};
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+39 -11
View File
@@ -38,7 +38,8 @@ using namespace time_literals;
DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_pure_pursuit_status_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
updateParams();
}
@@ -81,19 +82,46 @@ void DifferentialPosControl::updatePosControl()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
_current_state = DrivingState::SPOT_TURNING;
} else if (_current_state == DrivingState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
_current_state = DrivingState::DRIVING;
}
if (_current_state == DrivingState::SPOT_TURNING) {
speed_setpoint = 0.f; // stop during spot turning
} else if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(fabsf(heading_error),
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f,
_param_ro_max_thr_speed.get());
speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed);
}
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = speed_setpoint;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
yaw_setpoint + M_PI_F);
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = 0.f;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+19 -4
View File
@@ -46,7 +46,8 @@
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/pure_pursuit_status.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -54,6 +55,14 @@
using namespace matrix;
/**
* @brief Enum class for the different states of driving.
*/
enum class DrivingState {
SPOT_TURNING, // The vehicle is currently turning on the spot.
DRIVING // The vehicle is currently driving.
};
/**
* @brief Class for differential position control.
*/
@@ -68,7 +77,7 @@ public:
~DifferentialPosControl() = default;
/**
* @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
* @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint.
*/
void updatePosControl();
@@ -97,22 +106,28 @@ private:
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
// Variables
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
DrivingState _current_state{DrivingState::DRIVING};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
)
};
src/modules/rover_differential/DifferentialVelControl/CMakeLists.txt → src/modules/rover_differential/DifferentialSpeedControl/CMakeLists.txt
+4 -4
View File
@@ -31,9 +31,9 @@
#
############################################################################
px4_add_library(DifferentialVelControl
DifferentialVelControl.cpp
px4_add_library(DifferentialSpeedControl
DifferentialSpeedControl.cpp
)
target_link_libraries(DifferentialVelControl PUBLIC PID)
target_include_directories(DifferentialVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(DifferentialSpeedControl PUBLIC PID)
target_include_directories(DifferentialSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp → src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp
+36 -71
View File
@@ -31,19 +31,18 @@
*
****************************************************************************/
#include "DifferentialVelControl.hpp"
#include "DifferentialSpeedControl.hpp"
using namespace time_literals;
DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModuleParams(parent)
DifferentialSpeedControl::DifferentialSpeedControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_rover_speed_status_pub.advertise();
updateParams();
}
void DifferentialVelControl::updateParams()
void DifferentialSpeedControl::updateParams()
{
ModuleParams::updateParams();
@@ -58,34 +57,17 @@ void DifferentialVelControl::updateParams()
}
}
void DifferentialVelControl::updateVelControl()
void DifferentialSpeedControl::updateSpeedControl()
{
updateSubscriptions();
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
float max_speed = _param_ro_speed_limit.get();
// Attitude Setpoint
if (PX4_ISFINITE(_bearing_setpoint)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw));
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f, max_speed);
}
}
// Throttle Setpoint
if (PX4_ISFINITE(_speed_setpoint)) {
const float speed_setpoint = calcSpeedSetpoint(max_speed);
const float speed_setpoint = calcSpeedSetpoint();
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed,
@@ -93,28 +75,26 @@ void DifferentialVelControl::updateVelControl()
_param_ro_max_thr_speed.get(), dt);
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
// Publish velocity controller status (logging only)
rover_velocity_status_s rover_velocity_status;
rover_velocity_status.timestamp = _timestamp;
rover_velocity_status.measured_speed_body_x = _vehicle_speed;
rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState();
rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_velocity_status.measured_speed_body_y = NAN;
rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
rover_velocity_status.pid_throttle_body_y_integral = NAN;
_rover_velocity_status_pub.publish(rover_velocity_status);
// Publish speed controller status (logging only)
rover_speed_status_s rover_speed_status;
rover_speed_status.timestamp = _timestamp;
rover_speed_status.measured_speed_body_x = _vehicle_speed;
rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState();
rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
rover_speed_status.measured_speed_body_y = NAN;
rover_speed_status.adjusted_speed_body_y_setpoint = NAN;
rover_speed_status.pid_throttle_body_y_integral = NAN;
_rover_speed_status_pub.publish(rover_speed_status);
}
void DifferentialVelControl::updateSubscriptions()
void DifferentialSpeedControl::updateSubscriptions()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_vehicle_local_position_sub.updated()) {
@@ -126,64 +106,49 @@ void DifferentialVelControl::updateSubscriptions()
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_velocity_setpoint_sub.updated()) {
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
_speed_setpoint = rover_velocity_setpoint.speed;
_bearing_setpoint = rover_velocity_setpoint.bearing;
if (_rover_speed_setpoint_sub.updated()) {
rover_speed_setpoint_s rover_speed_setpoint;
_rover_speed_setpoint_sub.copy(&rover_speed_setpoint);
_speed_setpoint = rover_speed_setpoint.speed_body_x;
}
}
float DifferentialVelControl::calcSpeedSetpoint(const float max_speed)
float DifferentialSpeedControl::calcSpeedSetpoint()
{
const float heading_error = matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw);
float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
_current_state = DrivingState::SPOT_TURNING;
const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
} else if (_current_state == DrivingState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
_current_state = DrivingState::DRIVING;
if (_rover_steering_setpoint_sub.updated()) {
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
_normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint;
}
float speed_setpoint = 0.f;
if (_current_state == DrivingState::DRIVING) {
speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed);
const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
if (_rover_steering_setpoint_sub.updated()) {
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
_normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint;
}
if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
_normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
speed_setpoint = math::interpolate<float>(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f,
1.f,
- _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}
if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
_normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
speed_setpoint = math::interpolate<float>(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f,
1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}
return speed_setpoint;
}
bool DifferentialVelControl::runSanityChecks()
bool DifferentialSpeedControl::runSanityChecks()
{
bool ret = true;
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
ret = false;
events::send<float>(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error,
events::send<float>(events::ID("differential_speed_control_conf_invalid_speed_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
}
if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) {
ret = false;
events::send<float, float>(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error,
events::send<float, float>(events::ID("differential_speed_control_conf_invalid_speed_control"), events::Log::Error,
"Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup",
_param_ro_max_thr_speed.get(), _param_ro_speed_p.get());
}
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp → src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp
+16 -33
View File
@@ -48,9 +48,8 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_speed_status.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -58,30 +57,22 @@
using namespace matrix;
/**
* @brief Enum class for the different states of driving.
* @brief Class for differential speed control.
*/
enum class DrivingState {
SPOT_TURNING, // The vehicle is currently turning on the spot.
DRIVING // The vehicle is currently driving.
};
/**
* @brief Class for differential velocity control.
*/
class DifferentialVelControl : public ModuleParams
class DifferentialSpeedControl : public ModuleParams
{
public:
/**
* @brief Constructor for DifferentialVelControl.
* @brief Constructor for DifferentialSpeedControl.
* @param parent The parent ModuleParams object.
*/
DifferentialVelControl(ModuleParams *parent);
~DifferentialVelControl() = default;
DifferentialSpeedControl(ModuleParams *parent);
~DifferentialSpeedControl() = default;
/**
* @brief Generate and publish roverAttitudeSetpoint/RoverThrottleSetpoint from roverVelocitySetpoint.
* @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint.
*/
void updateVelControl();
void updateSpeedControl();
/**
* @brief Check if the necessary parameters are set.
@@ -90,9 +81,9 @@ public:
bool runSanityChecks();
/**
* @brief Reset velocity controller.
* @brief Reset speed controller.
*/
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
protected:
/**
@@ -102,45 +93,38 @@ protected:
private:
/**
* @brief Update uORB subscriptions used in velocity controller.
* @brief Update uORB subscriptions used in speed controller.
*/
void updateSubscriptions();
/**
* @brief Calculate the speed setpoint based on the current state.
* @param max_speed Maximum speed limit [m/s].
* @return Speed setpoint.
*/
float calcSpeedSetpoint(float max_speed);
float calcSpeedSetpoint();
// uORB subscriptions
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
// uORB publications
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_speed_status_s> _rover_speed_status_pub{ORB_ID(rover_speed_status)};
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f};
float _speed_setpoint{NAN};
float _bearing_setpoint{NAN};
float _normalized_speed_diff{NAN};
DrivingState _current_state{DrivingState::DRIVING};
// Controllers
PID _pid_speed;
SlewRate<float> _adjusted_speed_setpoint;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
@@ -148,7 +132,6 @@ private:
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
)
};
src/modules/rover_differential/RoverDifferential.cpp
+6 -6
View File
@@ -139,10 +139,6 @@ void RoverDifferential::updateControllers()
_differential_pos_control.updatePosControl();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_differential_vel_control.updateVelControl();
}
if (_vehicle_control_mode.flag_control_attitude_enabled) {
_differential_att_control.updateAttControl();
}
@@ -151,6 +147,10 @@ void RoverDifferential::updateControllers()
_differential_rate_control.updateRateControl();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_differential_speed_control.updateSpeedControl();
}
if (_vehicle_control_mode.flag_control_allocation_enabled) {
_differential_act_control.updateActControl();
}
@@ -168,7 +168,7 @@ void RoverDifferential::runSanityChecks()
return;
}
if (_vehicle_control_mode.flag_control_velocity_enabled && !_differential_vel_control.runSanityChecks()) {
if (_vehicle_control_mode.flag_control_velocity_enabled && !_differential_speed_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
@@ -183,7 +183,7 @@ void RoverDifferential::runSanityChecks()
void RoverDifferential::reset()
{
_differential_vel_control.reset();
_differential_speed_control.reset();
_differential_att_control.reset();
_differential_rate_control.reset();
_manual_mode.reset();
src/modules/rover_differential/RoverDifferential.hpp
+2 -2
View File
@@ -53,7 +53,7 @@
#include "DifferentialActControl/DifferentialActControl.hpp"
#include "DifferentialRateControl/DifferentialRateControl.hpp"
#include "DifferentialAttControl/DifferentialAttControl.hpp"
#include "DifferentialVelControl/DifferentialVelControl.hpp"
#include "DifferentialSpeedControl/DifferentialSpeedControl.hpp"
#include "DifferentialPosControl/DifferentialPosControl.hpp"
#include "DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp"
#include "DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp"
@@ -123,7 +123,7 @@ private:
DifferentialActControl _differential_act_control{this};
DifferentialRateControl _differential_rate_control{this};
DifferentialAttControl _differential_att_control{this};
DifferentialVelControl _differential_vel_control{this};
DifferentialSpeedControl _differential_speed_control{this};
DifferentialPosControl _differential_pos_control{this};
DifferentialAutoMode _auto_mode{this};
DifferentialManualMode _manual_mode{this};
src/modules/rover_mecanum/CMakeLists.txt
+2 -2
View File
@@ -34,7 +34,7 @@
add_subdirectory(MecanumActControl)
add_subdirectory(MecanumRateControl)
add_subdirectory(MecanumAttControl)
add_subdirectory(MecanumVelControl)
add_subdirectory(MecanumSpeedControl)
add_subdirectory(MecanumPosControl)
add_subdirectory(MecanumDriveModes)
@@ -48,7 +48,7 @@ px4_add_module(
MecanumActControl
MecanumRateControl
MecanumAttControl
MecanumVelControl
MecanumSpeedControl
MecanumPosControl
MecanumAutoMode
MecanumManualMode
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp
+8 -10
View File
@@ -42,7 +42,7 @@ MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent
_rover_steering_setpoint_pub.advertise();
_rover_rate_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
}
@@ -153,19 +153,16 @@ void MecanumManualMode::position()
velocity_setpoint_body(1) = math::interpolate<float>(manual_control_setpoint.roll,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
velocity_setpoint_body(2) = 0.f;
const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_ned.norm() < FLT_EPSILON) {
if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) {
_pos_ctl_yaw_setpoint = NAN;
// Speed control
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = velocity_setpoint_ned.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_ned(1), velocity_setpoint_ned(0));
rover_velocity_setpoint.yaw = NAN;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = velocity_setpoint_body(0);
rover_speed_setpoint.speed_body_y = velocity_setpoint_body(1);
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
@@ -192,6 +189,7 @@ void MecanumManualMode::position()
_rover_position_setpoint_pub.publish(rover_position_setpoint);
} else { // Course control
const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
const Vector3f pos_ctl_course_direction_local = velocity_setpoint_ned.normalized();
const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
pos_ctl_course_direction_local(1));
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
using namespace matrix;
@@ -110,7 +110,7 @@ private:
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables
src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp
+18 -7
View File
@@ -40,7 +40,7 @@ MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(pa
updateParams();
_rover_rate_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
}
@@ -70,12 +70,23 @@ void MecanumOffboardMode::offboardControl()
_rover_position_setpoint_pub.publish(rover_position_setpoint);
} else if (offboard_control_mode.velocity) {
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = hrt_absolute_time();
rover_velocity_setpoint.speed = velocity_ned.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
}
const Vector3f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], 0.f);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = hrt_absolute_time();
rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0);
rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1);
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = hrt_absolute_time();
rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else if (offboard_control_mode.attitude) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp
+6 -2
View File
@@ -45,10 +45,11 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
using namespace matrix;
@@ -80,10 +81,13 @@ private:
// uORB subscriptions
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
Quatf _vehicle_attitude_quaternion{};
};
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
+25 -13
View File
@@ -37,8 +37,9 @@ using namespace time_literals;
MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_velocity_setpoint_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_pure_pursuit_status_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
updateParams();
}
@@ -85,20 +86,31 @@ void MecanumPosControl::updatePosControl()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F);
rover_velocity_setpoint.yaw = _yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
const Vector3f velocity_in_local_frame(speed_setpoint * cosf(bearing_setpoint),
speed_setpoint * sinf(bearing_setpoint), 0.f);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0);
rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1);
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = 0.f;
rover_speed_setpoint.speed_body_y = 0.f;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
}
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+5 -3
View File
@@ -47,7 +47,8 @@
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -69,7 +70,7 @@ public:
~MecanumPosControl() = default;
/**
* @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
* @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint.
*/
void updatePosControl();
@@ -98,8 +99,9 @@ private:
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_speed_setpoint_s> _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
// Variables
Quatf _vehicle_attitude_quaternion{};
src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt → src/modules/rover_mecanum/MecanumSpeedControl/CMakeLists.txt
+5 -5
View File
@@ -31,10 +31,10 @@
#
############################################################################
px4_add_library(MecanumVelControl
MecanumVelControl.cpp
px4_add_library(MecanumSpeedControl
MecanumSpeedControl.cpp
)
target_link_libraries(MecanumVelControl PUBLIC PID)
target_link_libraries(MecanumVelControl PUBLIC pure_pursuit)
target_include_directories(MecanumVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(MecanumSpeedControl PUBLIC PID)
target_link_libraries(MecanumSpeedControl PUBLIC pure_pursuit)
target_include_directories(MecanumSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp → src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp
+22 -51
View File
@@ -31,19 +31,18 @@
*
****************************************************************************/
#include "MecanumVelControl.hpp"
#include "MecanumSpeedControl.hpp"
using namespace time_literals;
MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent)
MecanumSpeedControl::MecanumSpeedControl(ModuleParams *parent) : ModuleParams(parent)
{
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_rover_speed_status_pub.advertise();
updateParams();
}
void MecanumVelControl::updateParams()
void MecanumSpeedControl::updateParams()
{
ModuleParams::updateParams();
@@ -62,7 +61,7 @@ void MecanumVelControl::updateParams()
}
}
void MecanumVelControl::updateVelControl()
void MecanumSpeedControl::updateSpeedControl()
{
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
@@ -70,15 +69,6 @@ void MecanumVelControl::updateVelControl()
updateSubscriptions();
// Attitude Setpoint
if (PX4_ISFINITE(_yaw_setpoint)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
// Throttle Setpoints
if (PX4_ISFINITE(_speed_x_setpoint) && PX4_ISFINITE(_speed_y_setpoint)) {
Vector2f speed_setpoint = calcSpeedSetpoint();
@@ -96,18 +86,18 @@ void MecanumVelControl::updateVelControl()
}
// Publish position controller status (logging only)
rover_velocity_status_s rover_velocity_status;
rover_velocity_status.timestamp = _timestamp;
rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState();
rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
rover_velocity_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState();
rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral();
rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral();
_rover_velocity_status_pub.publish(rover_velocity_status);
rover_speed_status_s rover_speed_status;
rover_speed_status.timestamp = _timestamp;
rover_speed_status.measured_speed_body_x = _vehicle_speed_body_x;
rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState();
rover_speed_status.measured_speed_body_y = _vehicle_speed_body_y;
rover_speed_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState();
rover_speed_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral();
rover_speed_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral();
_rover_speed_status_pub.publish(rover_speed_status);
}
void MecanumVelControl::updateSubscriptions()
void MecanumSpeedControl::updateSubscriptions()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
@@ -125,34 +115,15 @@ void MecanumVelControl::updateSubscriptions()
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
}
if (_rover_velocity_setpoint_sub.updated()) {
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
const float speed_setpoint = math::constrain(rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
if (PX4_ISFINITE(rover_velocity_setpoint.speed) && PX4_ISFINITE(rover_velocity_setpoint.bearing)) {
const Vector3f velocity_in_local_frame(speed_setpoint * cosf(rover_velocity_setpoint.bearing),
speed_setpoint * sinf(rover_velocity_setpoint.bearing), 0.f);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_speed_x_setpoint = velocity_in_body_frame(0);
_speed_y_setpoint = velocity_in_body_frame(1);
} else if (PX4_ISFINITE(rover_velocity_setpoint.speed)) {
_speed_x_setpoint = speed_setpoint;
_speed_y_setpoint = 0.f;
} else {
_speed_x_setpoint = NAN;
_speed_y_setpoint = NAN;
}
_yaw_setpoint = rover_velocity_setpoint.yaw;
if (_rover_speed_setpoint_sub.updated()) {
rover_speed_setpoint_s rover_speed_setpoint;
_rover_speed_setpoint_sub.copy(&rover_speed_setpoint);
_speed_x_setpoint = rover_speed_setpoint.speed_body_x;
_speed_y_setpoint = rover_speed_setpoint.speed_body_y;
}
}
Vector2f MecanumVelControl::calcSpeedSetpoint()
Vector2f MecanumSpeedControl::calcSpeedSetpoint()
{
if (_rover_steering_setpoint_sub.updated()) {
rover_steering_setpoint_s rover_steering_setpoint{};
@@ -190,7 +161,7 @@ Vector2f MecanumVelControl::calcSpeedSetpoint()
return speed_setpoint;
}
bool MecanumVelControl::runSanityChecks()
bool MecanumSpeedControl::runSanityChecks()
{
bool ret = true;
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp → src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp
+14 -17
View File
@@ -49,31 +49,30 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_speed_status.h>
#include <uORB/topics/rover_speed_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace matrix;
/**
* @brief Class for mecanum velocity control.
* @brief Class for mecanum speed control.
*/
class MecanumVelControl : public ModuleParams
class MecanumSpeedControl : public ModuleParams
{
public:
/**
* @brief Constructor for MecanumVelControl.
* @brief Constructor for MecanumSpeedControl.
* @param parent The parent ModuleParams object.
*/
MecanumVelControl(ModuleParams *parent);
~MecanumVelControl() = default;
MecanumSpeedControl(ModuleParams *parent);
~MecanumSpeedControl() = default;
/**
* @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint.
* @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint.
*/
void updateVelControl();
void updateSpeedControl();
/**
* @brief Check if the necessary parameters are set.
@@ -82,9 +81,9 @@ public:
bool runSanityChecks();
/**
* @brief Reset velocity controller.
* @brief Reset speed controller.
*/
void reset() {_pid_speed_x.resetIntegral(); _pid_speed_y.resetIntegral(); _speed_x_setpoint = NAN; _speed_y_setpoint = NAN; _adjusted_speed_x_setpoint.setForcedValue(0.f); _adjusted_speed_y_setpoint.setForcedValue(0.f); _yaw_setpoint = NAN;};
void reset() {_pid_speed_x.resetIntegral(); _pid_speed_y.resetIntegral(); _speed_x_setpoint = NAN; _speed_y_setpoint = NAN; _adjusted_speed_x_setpoint.setForcedValue(0.f); _adjusted_speed_y_setpoint.setForcedValue(0.f);};
protected:
/**
@@ -94,7 +93,7 @@ protected:
private:
/**
* @brief Update uORB subscriptions used in velocity controller.
* @brief Update uORB subscriptions used in speed controller.
*/
void updateSubscriptions();
@@ -107,13 +106,12 @@ private:
// uORB subscriptions
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
// uORB publications
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_speed_status_s> _rover_speed_status_pub{ORB_ID(rover_speed_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -124,7 +122,6 @@ private:
float _speed_x_setpoint{NAN};
float _speed_y_setpoint{NAN};
float _normalized_speed_diff{NAN};
float _yaw_setpoint{NAN};
// Controllers
PID _pid_speed_x;
src/modules/rover_mecanum/RoverMecanum.cpp
+6 -6
View File
@@ -139,10 +139,6 @@ void RoverMecanum::updateControllers()
_mecanum_pos_control.updatePosControl();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_mecanum_vel_control.updateVelControl();
}
if (_vehicle_control_mode.flag_control_attitude_enabled) {
_mecanum_att_control.updateAttControl();
}
@@ -151,6 +147,10 @@ void RoverMecanum::updateControllers()
_mecanum_rate_control.updateRateControl();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_mecanum_speed_control.updateSpeedControl();
}
if (_vehicle_control_mode.flag_control_allocation_enabled) {
_mecanum_act_control.updateActControl();
}
@@ -168,7 +168,7 @@ void RoverMecanum::runSanityChecks()
return;
}
if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_vel_control.runSanityChecks()) {
if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_speed_control.runSanityChecks()) {
_sanity_checks_passed = false;
return;
}
@@ -183,7 +183,7 @@ void RoverMecanum::runSanityChecks()
void RoverMecanum::reset()
{
_mecanum_vel_control.reset();
_mecanum_speed_control.reset();
_mecanum_att_control.reset();
_mecanum_rate_control.reset();
_manual_mode.reset();
src/modules/rover_mecanum/RoverMecanum.hpp
+2 -2
View File
@@ -53,7 +53,7 @@
#include "MecanumActControl/MecanumActControl.hpp"
#include "MecanumRateControl/MecanumRateControl.hpp"
#include "MecanumAttControl/MecanumAttControl.hpp"
#include "MecanumVelControl/MecanumVelControl.hpp"
#include "MecanumSpeedControl/MecanumSpeedControl.hpp"
#include "MecanumPosControl/MecanumPosControl.hpp"
#include "MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp"
#include "MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp"
@@ -123,7 +123,7 @@ private:
MecanumActControl _mecanum_act_control{this};
MecanumRateControl _mecanum_rate_control{this};
MecanumAttControl _mecanum_att_control{this};
MecanumVelControl _mecanum_vel_control{this};
MecanumSpeedControl _mecanum_speed_control{this};
MecanumPosControl _mecanum_pos_control{this};
MecanumAutoMode _auto_mode{this};
MecanumManualMode _manual_mode{this};
src/modules/uxrce_dds_client/dds_topics.yaml
+2 -2
View File
@@ -199,8 +199,8 @@ subscriptions:
- topic: /fmu/in/rover_position_setpoint
type: px4_msgs::msg::RoverPositionSetpoint
- topic: /fmu/in/rover_velocity_setpoint
type: px4_msgs::msg::RoverVelocitySetpoint
- topic: /fmu/in/rover_speed_setpoint
type: px4_msgs::msg::RoverSpeedSetpoint
- topic: /fmu/in/rover_attitude_setpoint
type: px4_msgs::msg::RoverAttitudeSetpoint