mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mecanum: separate velocity control
This commit is contained in:
chfriedrich98
chfriedrich98
parent
3c39095271
commit
04512ee91f
@ -131,6 +131,7 @@ set(msg_files
|
||||
ManualControlSwitches.msg
|
||||
MavlinkLog.msg
|
||||
MavlinkTunnel.msg
|
||||
MecanumVelocitySetpoint.msg
|
||||
MessageFormatRequest.msg
|
||||
MessageFormatResponse.msg
|
||||
Mission.msg
|
||||
|
||||
5
msg/MecanumVelocitySetpoint.msg
Normal file
5
msg/MecanumVelocitySetpoint.msg
Normal file
@ -0,0 +1,5 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 speed # [m/s] [-inf, inf] Speed setpoint
|
||||
float32 bearing # [rad] [-pi, pi] from North.
|
||||
float32 yaw # [rad] [-pi, pi] (Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame
|
||||
@ -93,6 +93,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("magnetometer_bias_estimate", 200);
|
||||
add_topic("manual_control_setpoint", 200);
|
||||
add_topic("manual_control_switches");
|
||||
add_optional_topic("mecanum_velocity_setpoint", 100);
|
||||
add_topic("mission_result");
|
||||
add_topic("navigator_mission_item");
|
||||
add_topic("navigator_status");
|
||||
|
||||
@ -33,7 +33,8 @@
|
||||
|
||||
add_subdirectory(MecanumRateControl)
|
||||
add_subdirectory(MecanumAttControl)
|
||||
add_subdirectory(MecanumPosVelControl)
|
||||
add_subdirectory(MecanumVelControl)
|
||||
add_subdirectory(MecanumPosControl)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__rover_mecanum
|
||||
@ -44,7 +45,8 @@ px4_add_module(
|
||||
DEPENDS
|
||||
MecanumRateControl
|
||||
MecanumAttControl
|
||||
MecanumPosVelControl
|
||||
MecanumVelControl
|
||||
MecanumPosControl
|
||||
px4_work_queue
|
||||
rover_control
|
||||
pure_pursuit
|
||||
|
||||
@ -92,7 +92,7 @@ void MecanumAttControl::updateAttControl()
|
||||
rover_attitude_status_s rover_attitude_status;
|
||||
rover_attitude_status.timestamp = _timestamp;
|
||||
rover_attitude_status.measured_yaw = _vehicle_yaw;
|
||||
rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState();
|
||||
rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState());
|
||||
_rover_attitude_status_pub.publish(rover_attitude_status);
|
||||
|
||||
}
|
||||
|
||||
@ -126,7 +126,7 @@ private:
|
||||
float _vehicle_yaw{0.f};
|
||||
float _dt{0.f};
|
||||
float _max_yaw_rate{0.f};
|
||||
float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
|
||||
float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
|
||||
bool _prev_param_check_passed{true};
|
||||
|
||||
// Controllers
|
||||
|
||||
@ -31,10 +31,10 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(MecanumPosVelControl
|
||||
MecanumPosVelControl.cpp
|
||||
px4_add_library(MecanumPosControl
|
||||
MecanumPosControl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(MecanumPosVelControl PUBLIC PID)
|
||||
target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit)
|
||||
target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(MecanumPosControl PUBLIC PID)
|
||||
target_link_libraries(MecanumPosControl PUBLIC pure_pursuit)
|
||||
target_include_directories(MecanumPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@ -0,0 +1,351 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 "MecanumPosControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_mecanum_velocity_setpoint_pub.advertise();
|
||||
_rover_position_setpoint_pub.advertise();
|
||||
_pure_pursuit_status_pub.advertise();
|
||||
|
||||
// Initially set to NaN to indicate that the rover has no position setpoint
|
||||
_rover_position_setpoint.position_ned[0] = NAN;
|
||||
_rover_position_setpoint.position_ned[1] = NAN;
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void MecanumPosControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
|
||||
}
|
||||
|
||||
void MecanumPosControl::updatePosControl()
|
||||
{
|
||||
const hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
updateSubscriptions();
|
||||
|
||||
if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
|
||||
if (_vehicle_control_mode.flag_control_offboard_enabled) {
|
||||
generatePositionSetpoint();
|
||||
}
|
||||
|
||||
generateVelocitySetpoint();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosControl::updateSubscriptions()
|
||||
{
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
|
||||
}
|
||||
|
||||
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()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void MecanumPosControl::generatePositionSetpoint()
|
||||
{
|
||||
if (_offboard_control_mode_sub.updated()) {
|
||||
_offboard_control_mode_sub.copy(&_offboard_control_mode);
|
||||
}
|
||||
|
||||
if (!_offboard_control_mode.position) {
|
||||
return;
|
||||
}
|
||||
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
// Translate trajectory setpoint to rover position setpoint
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = _timestamp;
|
||||
rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
|
||||
rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
|
||||
rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get();
|
||||
rover_position_setpoint.yaw = _vehicle_yaw;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
|
||||
}
|
||||
|
||||
void MecanumPosControl::generateVelocitySetpoint()
|
||||
{
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) {
|
||||
manualPositionMode();
|
||||
|
||||
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
|
||||
autoPositionMode();
|
||||
|
||||
} else if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint)
|
||||
&& PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) {
|
||||
goToPositionMode();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void MecanumPosControl::manualPositionMode()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
|
||||
Vector3f velocity_setpoint_body{};
|
||||
velocity_setpoint_body(0) = math::interpolate<float>(manual_control_setpoint.throttle,
|
||||
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
|
||||
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());
|
||||
const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
|
||||
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
|
||||
_max_yaw_rate / _param_ro_yaw_p.get());
|
||||
|
||||
if (fabsf(yaw_delta) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_pos_ctl_yaw_setpoint = NAN;
|
||||
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
|
||||
const Vector3f velocity_setpoint_local = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = velocity_setpoint_body.norm();
|
||||
mecanum_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0));
|
||||
mecanum_velocity_setpoint.yaw = yaw_setpoint;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
|
||||
} else { // Course control if the steering input is zero (keep driving on a straight line)
|
||||
const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f);
|
||||
const float velocity_magnitude_setpoint = velocity.norm();
|
||||
const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
|
||||
const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
|
||||
pos_ctl_course_direction_local(1));
|
||||
|
||||
// Reset course control if course direction change is above threshold
|
||||
if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp(
|
||||
1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) {
|
||||
_pos_ctl_yaw_setpoint = NAN;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
|
||||
_pos_ctl_start_position_ned = _curr_pos_ned;
|
||||
_pos_ctl_yaw_setpoint = _vehicle_yaw;
|
||||
_pos_ctl_course_direction = pos_ctl_course_direction_temp;
|
||||
}
|
||||
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
|
||||
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
|
||||
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
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, _pos_ctl_start_position_ned,
|
||||
_curr_pos_ned, velocity_magnitude_setpoint);
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = velocity_magnitude_setpoint;
|
||||
mecanum_velocity_setpoint.bearing = bearing_setpoint;
|
||||
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosControl::autoPositionMode()
|
||||
{
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
position_setpoint_triplet_s position_setpoint_triplet{};
|
||||
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
|
||||
_curr_wp_type = position_setpoint_triplet.current.type;
|
||||
|
||||
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
|
||||
_curr_pos_ned, _global_ned_proj_ref);
|
||||
|
||||
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
|
||||
|
||||
// Waypoint cruising speed
|
||||
_auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
|
||||
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
|
||||
|
||||
// Waypoint yaw setpoint
|
||||
if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
|
||||
_auto_yaw = position_setpoint_triplet.current.yaw;
|
||||
|
||||
} else {
|
||||
_auto_yaw = _vehicle_yaw;
|
||||
}
|
||||
}
|
||||
|
||||
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
|
||||
|
||||
// Check stopping conditions
|
||||
bool auto_stop{false};
|
||||
|
||||
if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
|
||||
|| !_next_wp_ned.isAllFinite()) { // Check stopping conditions
|
||||
auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
if (auto_stop) {
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = 0.f;
|
||||
mecanum_velocity_setpoint.bearing = 0.f;
|
||||
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
|
||||
} else { // Regular guidance algorithm
|
||||
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
|
||||
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
|
||||
_curr_wp_type);
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
|
||||
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
|
||||
velocity_magnitude);
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = velocity_magnitude;
|
||||
mecanum_velocity_setpoint.bearing = bearing_setpoint;
|
||||
mecanum_velocity_setpoint.yaw = _auto_yaw;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
float MecanumPosControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp,
|
||||
const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
|
||||
const float miss_spd_gain, const int curr_wp_type)
|
||||
{
|
||||
// Upcoming stop
|
||||
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
|
||||
const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_decel, distance_to_curr_wp, 0.f);
|
||||
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
|
||||
}
|
||||
|
||||
// Straight line speed
|
||||
if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
|
||||
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
|
||||
M_PI_F, 0.f, 1.f), 0.f, 1.f);
|
||||
const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
|
||||
distance_to_curr_wp,
|
||||
max_speed * (1.f - speed_reduction));
|
||||
|
||||
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
|
||||
}
|
||||
|
||||
return auto_speed; // Fallthrough
|
||||
}
|
||||
|
||||
void MecanumPosControl::goToPositionMode()
|
||||
{
|
||||
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
|
||||
const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
|
||||
|
||||
if (distance_to_target > _param_nav_acc_rad.get()) {
|
||||
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
|
||||
_param_ro_decel_limit.get(), distance_to_target, 0.f);
|
||||
const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ?
|
||||
_rover_position_setpoint.cruising_speed :
|
||||
_param_ro_speed_limit.get();
|
||||
speed_setpoint = math::min(speed_setpoint, max_speed);
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
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, _curr_pos_ned,
|
||||
_curr_pos_ned, fabsf(speed_setpoint));
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = speed_setpoint;
|
||||
mecanum_velocity_setpoint.bearing = bearing_setpoint;
|
||||
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
|
||||
} else {
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = 0.f;
|
||||
mecanum_velocity_setpoint.bearing = 0.f;
|
||||
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
bool MecanumPosControl::runSanityChecks()
|
||||
{
|
||||
bool ret = true;
|
||||
|
||||
if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
}
|
||||
|
||||
_prev_param_check_passed = ret;
|
||||
return ret;
|
||||
}
|
||||
@ -49,11 +49,8 @@
|
||||
// uORB includes
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/rover_rate_setpoint.h>
|
||||
#include <uORB/topics/rover_throttle_setpoint.h>
|
||||
#include <uORB/topics/rover_velocity_status.h>
|
||||
#include <uORB/topics/rover_attitude_setpoint.h>
|
||||
#include <uORB/topics/rover_steering_setpoint.h>
|
||||
#include <uORB/topics/mecanum_velocity_setpoint.h>
|
||||
#include <uORB/topics/rover_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
@ -67,17 +64,17 @@
|
||||
using namespace matrix;
|
||||
|
||||
/**
|
||||
* @brief Class for mecanum position/velocity control.
|
||||
* @brief Class for mecanum position control.
|
||||
*/
|
||||
class MecanumPosVelControl : public ModuleParams
|
||||
class MecanumPosControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for MecanumPosVelControl.
|
||||
* @brief Constructor for MecanumPosControl.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
MecanumPosVelControl(ModuleParams *parent);
|
||||
~MecanumPosVelControl() = default;
|
||||
MecanumPosControl(ModuleParams *parent);
|
||||
~MecanumPosControl() = default;
|
||||
|
||||
/**
|
||||
* @brief Update position controller.
|
||||
@ -97,32 +94,36 @@ private:
|
||||
void updateSubscriptions();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or
|
||||
* trajcetorySetpoint (Offboard position or velocity control) or
|
||||
* positionSetpointTriplet (Auto Mode).
|
||||
* @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint.
|
||||
*/
|
||||
void generateAttitudeSetpoint();
|
||||
void generatePositionSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint.
|
||||
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or
|
||||
* positionSetpointTriplet (Auto Mode) or roverPositionSetpoint.
|
||||
*/
|
||||
void generateVelocitySetpoint();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manualPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint.
|
||||
*/
|
||||
void offboardPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint.
|
||||
*/
|
||||
void offboardVelocityMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet.
|
||||
* @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
|
||||
*/
|
||||
void autoPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
|
||||
*/
|
||||
void goToPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to
|
||||
* Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
|
||||
@ -155,17 +156,16 @@ private:
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
|
||||
uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
offboard_control_mode_s _offboard_control_mode{};
|
||||
rover_steering_setpoint_s _rover_steering_setpoint{};
|
||||
rover_position_setpoint_s _rover_position_setpoint{};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
|
||||
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<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
|
||||
uORB::Publication<mecanum_velocity_setpoint_s> _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)};
|
||||
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
|
||||
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
|
||||
|
||||
|
||||
// Variables
|
||||
hrt_abstime _timestamp{0};
|
||||
@ -173,12 +173,8 @@ private:
|
||||
Vector2f _curr_pos_ned{};
|
||||
Vector2f _pos_ctl_course_direction{};
|
||||
Vector2f _pos_ctl_start_position_ned{};
|
||||
float _vehicle_speed_body_x{0.f};
|
||||
float _vehicle_speed_body_y{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
float _max_yaw_rate{0.f};
|
||||
float _speed_body_x_setpoint{0.f};
|
||||
float _speed_body_y_setpoint{0.f};
|
||||
float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad]
|
||||
float _dt{0.f};
|
||||
float _auto_speed{0.f};
|
||||
@ -190,14 +186,9 @@ private:
|
||||
Vector2f _curr_wp_ned{};
|
||||
Vector2f _prev_wp_ned{};
|
||||
Vector2f _next_wp_ned{};
|
||||
float _cruising_speed{0.f};
|
||||
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
|
||||
// Controllers
|
||||
PID _pid_speed_x;
|
||||
PID _pid_speed_y;
|
||||
SlewRate<float> _speed_x_setpoint;
|
||||
SlewRate<float> _speed_y_setpoint;
|
||||
|
||||
// Class Instances
|
||||
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
|
||||
|
||||
@ -217,6 +208,7 @@ private:
|
||||
(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_P>) _param_ro_yaw_p,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
|
||||
|
||||
)
|
||||
@ -1,432 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 "MecanumPosVelControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_rover_rate_setpoint_pub.advertise();
|
||||
_rover_throttle_setpoint_pub.advertise();
|
||||
_rover_attitude_setpoint_pub.advertise();
|
||||
_rover_velocity_status_pub.advertise();
|
||||
_pure_pursuit_status_pub.advertise();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
|
||||
_pid_speed_x.setIntegralLimit(1.f);
|
||||
_pid_speed_x.setOutputLimit(1.f);
|
||||
_pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
|
||||
_pid_speed_y.setIntegralLimit(1.f);
|
||||
_pid_speed_y.setOutputLimit(1.f);
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
|
||||
if (_param_ro_accel_limit.get() > FLT_EPSILON) {
|
||||
_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get());
|
||||
_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get());
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::updatePosControl()
|
||||
{
|
||||
const hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
updateSubscriptions();
|
||||
|
||||
if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled)
|
||||
&& _vehicle_control_mode.flag_armed && runSanityChecks()) {
|
||||
generateAttitudeSetpoint();
|
||||
|
||||
if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible
|
||||
if (_rover_steering_setpoint_sub.updated()) {
|
||||
_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
|
||||
}
|
||||
|
||||
float speed_body_x_setpoint_normalized = math::interpolate<float>(_speed_body_x_setpoint,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
|
||||
|
||||
float speed_body_y_setpoint_normalized = math::interpolate<float>(_speed_body_y_setpoint,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
|
||||
|
||||
const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf(
|
||||
_rover_steering_setpoint.normalized_speed_diff);
|
||||
|
||||
if (total_speed > 1.f) {
|
||||
const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized));
|
||||
const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta));
|
||||
const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized,
|
||||
2.f) + powf(speed_body_y_setpoint_normalized, 2.f)));
|
||||
speed_body_x_setpoint_normalized *= magnitude * normalization;
|
||||
speed_body_y_setpoint_normalized *= magnitude * normalization;
|
||||
_speed_body_x_setpoint = math::interpolate<float>(speed_body_x_setpoint_normalized, -1.f, 1.f,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
|
||||
_speed_body_y_setpoint = math::interpolate<float>(speed_body_y_setpoint_normalized, -1.f, 1.f,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
|
||||
}
|
||||
}
|
||||
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = _timestamp;
|
||||
_speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f;
|
||||
_speed_body_y_setpoint = fabsf(_speed_body_y_setpoint) > _param_ro_speed_th.get() ? _speed_body_y_setpoint : 0.f;
|
||||
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x,
|
||||
_speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
|
||||
_param_ro_max_thr_speed.get(), _dt);
|
||||
rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y,
|
||||
_speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
|
||||
_param_ro_max_thr_speed.get(), _dt);
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
|
||||
} else { // Reset controller and slew rate when position control is not active
|
||||
_pid_speed_x.resetIntegral();
|
||||
_speed_x_setpoint.setForcedValue(0.f);
|
||||
_pid_speed_y.resetIntegral();
|
||||
_speed_y_setpoint.setForcedValue(0.f);
|
||||
}
|
||||
|
||||
// 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.speed_body_x_setpoint = _speed_body_x_setpoint;
|
||||
rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState();
|
||||
rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
|
||||
rover_velocity_status.speed_body_y_setpoint = _speed_body_y_setpoint;
|
||||
rover_velocity_status.adjusted_speed_body_y_setpoint = _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);
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::updateSubscriptions()
|
||||
{
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
|
||||
}
|
||||
|
||||
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()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
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_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
|
||||
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::generateAttitudeSetpoint()
|
||||
{
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled
|
||||
&& _vehicle_control_mode.flag_control_position_enabled) { // Position Mode
|
||||
manualPositionMode();
|
||||
|
||||
} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control
|
||||
if (_offboard_control_mode_sub.updated()) {
|
||||
_offboard_control_mode_sub.copy(&_offboard_control_mode);
|
||||
}
|
||||
|
||||
if (_offboard_control_mode.position) {
|
||||
offboardPositionMode();
|
||||
|
||||
} else if (_offboard_control_mode.velocity) {
|
||||
offboardVelocityMode();
|
||||
}
|
||||
|
||||
} else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode
|
||||
autoPositionMode();
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::manualPositionMode()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
_speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
|
||||
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
|
||||
_speed_body_y_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
|
||||
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
|
||||
const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
|
||||
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
|
||||
if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_pos_ctl_yaw_setpoint = NAN;
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = _timestamp;
|
||||
rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint;
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
|
||||
} else if ((fabsf(_speed_body_x_setpoint) > FLT_EPSILON
|
||||
|| fabsf(_speed_body_y_setpoint) >
|
||||
FLT_EPSILON)) { // Course control if the steering input is zero (keep driving on a straight line)
|
||||
const Vector3f velocity = Vector3f(_speed_body_x_setpoint, _speed_body_y_setpoint, 0.f);
|
||||
const float velocity_magnitude_setpoint = velocity.norm();
|
||||
const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
|
||||
const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
|
||||
pos_ctl_course_direction_local(1));
|
||||
|
||||
// Reset course control if course direction change is above threshold
|
||||
if (fabsf(asinf(pos_ctl_course_direction_temp % _pos_ctl_course_direction)) > _param_rm_course_ctl_th.get()) {
|
||||
_pos_ctl_yaw_setpoint = NAN;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
|
||||
_pos_ctl_start_position_ned = _curr_pos_ned;
|
||||
_pos_ctl_yaw_setpoint = _vehicle_yaw;
|
||||
_pos_ctl_course_direction = pos_ctl_course_direction_temp;
|
||||
}
|
||||
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
|
||||
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
|
||||
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
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, _pos_ctl_start_position_ned,
|
||||
_curr_pos_ned, velocity_magnitude_setpoint);
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
|
||||
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
|
||||
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = _timestamp;
|
||||
rover_attitude_setpoint.yaw_setpoint = _pos_ctl_yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
|
||||
} else { // Reset course control and yaw rate setpoint
|
||||
_pos_ctl_yaw_setpoint = NAN;
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = _timestamp;
|
||||
rover_rate_setpoint.yaw_rate_setpoint = 0.f;
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::offboardPositionMode()
|
||||
{
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
// Translate trajectory setpoint to rover setpoints
|
||||
const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]);
|
||||
const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
|
||||
|
||||
if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) {
|
||||
const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(
|
||||
_param_ro_jerk_limit.get(),
|
||||
_param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get());
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
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, _curr_pos_ned,
|
||||
_curr_pos_ned, velocity_magnitude_setpoint);
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
|
||||
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
|
||||
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
|
||||
|
||||
} else {
|
||||
_speed_body_x_setpoint = 0.f;
|
||||
_speed_body_y_setpoint = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::offboardVelocityMode()
|
||||
{
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
const Vector3f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1],
|
||||
trajectory_setpoint.velocity[2]);
|
||||
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
|
||||
if (velocity_in_body_frame.isAllFinite()) {
|
||||
_speed_body_x_setpoint = velocity_in_body_frame(0);
|
||||
_speed_body_y_setpoint = velocity_in_body_frame(1);
|
||||
|
||||
} else {
|
||||
_speed_body_x_setpoint = 0.f;
|
||||
_speed_body_y_setpoint = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumPosVelControl::autoPositionMode()
|
||||
{
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
position_setpoint_triplet_s position_setpoint_triplet{};
|
||||
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
|
||||
_curr_wp_type = position_setpoint_triplet.current.type;
|
||||
|
||||
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
|
||||
_curr_pos_ned, _global_ned_proj_ref);
|
||||
|
||||
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
|
||||
|
||||
// Waypoint cruising speed
|
||||
_auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
|
||||
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
|
||||
|
||||
// Waypoint yaw setpoint
|
||||
if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
|
||||
_auto_yaw = position_setpoint_triplet.current.yaw;
|
||||
|
||||
} else {
|
||||
_auto_yaw = _vehicle_yaw;
|
||||
}
|
||||
}
|
||||
|
||||
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
|
||||
|
||||
// Check stopping conditions
|
||||
bool auto_stop{false};
|
||||
|
||||
if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
|
||||
|| !_next_wp_ned.isAllFinite()) { // Check stopping conditions
|
||||
auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
if (auto_stop) {
|
||||
_speed_body_x_setpoint = 0.f;
|
||||
_speed_body_y_setpoint = 0.f;
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = _timestamp;
|
||||
rover_rate_setpoint.yaw_rate_setpoint = 0.f;
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
|
||||
} else { // Regular guidance algorithm
|
||||
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
|
||||
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
|
||||
_curr_wp_type);
|
||||
pure_pursuit_status_s pure_pursuit_status{};
|
||||
pure_pursuit_status.timestamp = _timestamp;
|
||||
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
|
||||
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
|
||||
velocity_magnitude);
|
||||
_pure_pursuit_status_pub.publish(pure_pursuit_status);
|
||||
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
|
||||
Vector2f desired_velocity(0.f, 0.f);
|
||||
_speed_body_x_setpoint = velocity_magnitude * cosf(bearing_setpoint_body_frame);
|
||||
_speed_body_y_setpoint = velocity_magnitude * sinf(bearing_setpoint_body_frame);
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = _timestamp;
|
||||
rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp,
|
||||
const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
|
||||
const float miss_spd_gain, const int curr_wp_type)
|
||||
{
|
||||
// Upcoming stop
|
||||
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
|
||||
const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
|
||||
max_decel, distance_to_curr_wp, 0.f);
|
||||
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
|
||||
}
|
||||
|
||||
// Straight line speed
|
||||
if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
|
||||
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
|
||||
M_PI_F, 0.f, 1.f), 0.f, 1.f);
|
||||
const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
|
||||
distance_to_curr_wp,
|
||||
max_speed * (1.f - speed_reduction));
|
||||
|
||||
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
|
||||
}
|
||||
|
||||
return auto_speed; // Fallthrough
|
||||
}
|
||||
|
||||
bool MecanumPosVelControl::runSanityChecks()
|
||||
{
|
||||
bool ret = true;
|
||||
|
||||
if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
|
||||
if (_prev_param_check_passed) {
|
||||
events::send<float>(events::ID("mecanum_posVel_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;
|
||||
|
||||
if (_prev_param_check_passed) {
|
||||
events::send<float, float>(events::ID("mecanum_posVel_control_conf_invalid_speed_control"), events::Log::Error,
|
||||
"Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPD) nor feedback (RO_SPEED_P) is setup",
|
||||
_param_ro_max_thr_speed.get(),
|
||||
_param_ro_speed_p.get());
|
||||
}
|
||||
}
|
||||
|
||||
_prev_param_check_passed = ret;
|
||||
return ret;
|
||||
}
|
||||
40
src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt
Normal file
40
src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt
Normal file
@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 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(MecanumVelControl
|
||||
MecanumVelControl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(MecanumVelControl PUBLIC PID)
|
||||
target_link_libraries(MecanumVelControl PUBLIC pure_pursuit)
|
||||
target_include_directories(MecanumVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@ -0,0 +1,249 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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 "MecanumVelControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_rover_throttle_setpoint_pub.advertise();
|
||||
_rover_attitude_setpoint_pub.advertise();
|
||||
_mecanum_velocity_setpoint_pub.advertise();
|
||||
_rover_velocity_status_pub.advertise();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void MecanumVelControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
|
||||
_pid_speed_x.setIntegralLimit(1.f);
|
||||
_pid_speed_x.setOutputLimit(1.f);
|
||||
_pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
|
||||
_pid_speed_y.setIntegralLimit(1.f);
|
||||
_pid_speed_y.setOutputLimit(1.f);
|
||||
|
||||
if (_param_ro_accel_limit.get() > FLT_EPSILON) {
|
||||
_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get());
|
||||
_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get());
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumVelControl::updateVelControl()
|
||||
{
|
||||
const hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
updateSubscriptions();
|
||||
|
||||
if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) {
|
||||
if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control
|
||||
generateVelocitySetpoint();
|
||||
}
|
||||
|
||||
generateAttitudeAndThrottleSetpoint();
|
||||
|
||||
} else { // Reset controller and slew rate when velocity control is not active
|
||||
_pid_speed_x.resetIntegral();
|
||||
_speed_x_setpoint.setForcedValue(0.f);
|
||||
_pid_speed_y.resetIntegral();
|
||||
_speed_y_setpoint.setForcedValue(0.f);
|
||||
}
|
||||
|
||||
// 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 = _speed_x_setpoint.getState();
|
||||
rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
|
||||
rover_velocity_status.adjusted_speed_body_y_setpoint = _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);
|
||||
}
|
||||
|
||||
void MecanumVelControl::updateSubscriptions()
|
||||
{
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
|
||||
}
|
||||
|
||||
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()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
||||
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
|
||||
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
|
||||
}
|
||||
|
||||
}
|
||||
void MecanumVelControl::generateVelocitySetpoint()
|
||||
{
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
if (_offboard_control_mode_sub.updated()) {
|
||||
_offboard_control_mode_sub.copy(&_offboard_control_mode);
|
||||
}
|
||||
|
||||
const bool offboard_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position;
|
||||
|
||||
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
|
||||
|
||||
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
|
||||
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
|
||||
mecanum_velocity_setpoint.timestamp = _timestamp;
|
||||
mecanum_velocity_setpoint.speed = velocity_in_local_frame.norm();
|
||||
mecanum_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
|
||||
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
|
||||
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
|
||||
{
|
||||
if (_mecanum_velocity_setpoint_sub.updated()) {
|
||||
_mecanum_velocity_setpoint_sub.copy(&_mecanum_velocity_setpoint);
|
||||
}
|
||||
|
||||
// Attitude Setpoint
|
||||
if (PX4_ISFINITE(_mecanum_velocity_setpoint.yaw)) {
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = _timestamp;
|
||||
rover_attitude_setpoint.yaw_setpoint = _mecanum_velocity_setpoint.yaw;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
_last_attitude_setpoint_update = _timestamp;
|
||||
|
||||
} else {
|
||||
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);
|
||||
_last_attitude_setpoint_update = _timestamp;
|
||||
}
|
||||
|
||||
// Throttle Setpoint
|
||||
float speed_body_x_setpoint{0.f};
|
||||
float speed_body_y_setpoint{0.f};
|
||||
|
||||
if (fabsf(_mecanum_velocity_setpoint.speed) > FLT_EPSILON) {
|
||||
const Vector3f velocity_in_local_frame(_mecanum_velocity_setpoint.speed * cosf(
|
||||
_mecanum_velocity_setpoint.bearing),
|
||||
_mecanum_velocity_setpoint.speed * sinf(_mecanum_velocity_setpoint.bearing), 0.f);
|
||||
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
speed_body_x_setpoint = velocity_in_body_frame(0);
|
||||
speed_body_y_setpoint = velocity_in_body_frame(1);
|
||||
|
||||
}
|
||||
|
||||
if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible
|
||||
if (_rover_steering_setpoint_sub.updated()) {
|
||||
_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
|
||||
}
|
||||
|
||||
float speed_body_x_setpoint_normalized = math::interpolate<float>(speed_body_x_setpoint,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
|
||||
|
||||
float speed_body_y_setpoint_normalized = math::interpolate<float>(speed_body_y_setpoint,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
|
||||
|
||||
const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf(
|
||||
_rover_steering_setpoint.normalized_speed_diff);
|
||||
|
||||
if (total_speed > 1.f) {
|
||||
const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized));
|
||||
const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta));
|
||||
const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized,
|
||||
2.f) + powf(speed_body_y_setpoint_normalized, 2.f)));
|
||||
speed_body_x_setpoint_normalized *= magnitude * normalization;
|
||||
speed_body_y_setpoint_normalized *= magnitude * normalization;
|
||||
speed_body_x_setpoint = math::interpolate<float>(speed_body_x_setpoint_normalized, -1.f, 1.f,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
|
||||
speed_body_y_setpoint = math::interpolate<float>(speed_body_y_setpoint_normalized, -1.f, 1.f,
|
||||
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
|
||||
}
|
||||
}
|
||||
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = _timestamp;
|
||||
speed_body_x_setpoint = fabsf(speed_body_x_setpoint) > _param_ro_speed_th.get() ? speed_body_x_setpoint : 0.f;
|
||||
speed_body_y_setpoint = fabsf(speed_body_y_setpoint) > _param_ro_speed_th.get() ? speed_body_y_setpoint : 0.f;
|
||||
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x,
|
||||
speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
|
||||
_param_ro_max_thr_speed.get(), _dt);
|
||||
rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y,
|
||||
speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
|
||||
_param_ro_max_thr_speed.get(), _dt);
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
|
||||
}
|
||||
|
||||
bool MecanumVelControl::runSanityChecks()
|
||||
{
|
||||
bool ret = true;
|
||||
|
||||
if (_param_ro_speed_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
|
||||
if (_prev_param_check_passed) {
|
||||
events::send<float>(events::ID("mecanum_vel_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;
|
||||
|
||||
if (_prev_param_check_passed) {
|
||||
events::send<float, float>(events::ID("mecanum_vel_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());
|
||||
}
|
||||
}
|
||||
|
||||
_prev_param_check_passed = ret;
|
||||
return ret;
|
||||
}
|
||||
@ -0,0 +1,157 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
// Libraries
|
||||
#include <lib/rover_control/RoverControl.hpp>
|
||||
#include <lib/pid/PID.hpp>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <math.h>
|
||||
|
||||
// uORB includes
|
||||
#include <uORB/Publication.hpp>
|
||||
#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/mecanum_velocity_setpoint.h>
|
||||
#include <uORB/topics/rover_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
/**
|
||||
* @brief Class for mecanum velocity control.
|
||||
*/
|
||||
class MecanumVelControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for MecanumVelControl.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
MecanumVelControl(ModuleParams *parent);
|
||||
~MecanumVelControl() = default;
|
||||
|
||||
/**
|
||||
* @brief Update velocity controller.
|
||||
*/
|
||||
void updateVelControl();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Update uORB subscriptions used in velocity controller.
|
||||
*/
|
||||
void updateSubscriptions();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint.
|
||||
*/
|
||||
void generateVelocitySetpoint();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint
|
||||
* from roverVelocitySetpoint.
|
||||
*/
|
||||
void generateAttitudeAndThrottleSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Check if the necessary parameters are set.
|
||||
* @return True if all checks pass.
|
||||
*/
|
||||
bool runSanityChecks();
|
||||
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
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::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _mecanum_velocity_setpoint_sub{ORB_ID(mecanum_velocity_setpoint)};
|
||||
uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
|
||||
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
offboard_control_mode_s _offboard_control_mode{};
|
||||
rover_steering_setpoint_s _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<mecanum_velocity_setpoint_s> _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)};
|
||||
mecanum_velocity_setpoint_s _mecanum_velocity_setpoint{};
|
||||
|
||||
// Variables
|
||||
hrt_abstime _timestamp{0};
|
||||
hrt_abstime _last_attitude_setpoint_update{0};
|
||||
Quatf _vehicle_attitude_quaternion{};
|
||||
float _vehicle_speed_body_x{0.f};
|
||||
float _vehicle_speed_body_y{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
float _dt{0.f};
|
||||
bool _prev_param_check_passed{false};
|
||||
|
||||
// Controllers
|
||||
PID _pid_speed_x;
|
||||
PID _pid_speed_y;
|
||||
SlewRate<float> _speed_x_setpoint;
|
||||
SlewRate<float> _speed_y_setpoint;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(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,
|
||||
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
|
||||
(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
|
||||
|
||||
)
|
||||
};
|
||||
@ -70,7 +70,8 @@ void RoverMecanum::Run()
|
||||
_timestamp = hrt_absolute_time();
|
||||
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
_mecanum_pos_vel_control.updatePosControl();
|
||||
_mecanum_pos_control.updatePosControl();
|
||||
_mecanum_vel_control.updateVelControl();
|
||||
_mecanum_att_control.updateAttControl();
|
||||
_mecanum_rate_control.updateRateControl();
|
||||
|
||||
@ -83,7 +84,7 @@ void RoverMecanum::Run()
|
||||
&& !_vehicle_control_mode.flag_control_rates_enabled;
|
||||
|
||||
if (full_manual_mode_enabled) { // Manual mode
|
||||
generateSteeringSetpoint();
|
||||
generateSteeringAndThrottleSetpoint();
|
||||
}
|
||||
|
||||
if (_vehicle_control_mode.flag_armed) {
|
||||
@ -93,7 +94,7 @@ void RoverMecanum::Run()
|
||||
|
||||
}
|
||||
|
||||
void RoverMecanum::generateSteeringSetpoint()
|
||||
void RoverMecanum::generateSteeringAndThrottleSetpoint()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
|
||||
@ -58,7 +58,8 @@
|
||||
// Local includes
|
||||
#include "MecanumRateControl/MecanumRateControl.hpp"
|
||||
#include "MecanumAttControl/MecanumAttControl.hpp"
|
||||
#include "MecanumPosVelControl/MecanumPosVelControl.hpp"
|
||||
#include "MecanumVelControl/MecanumVelControl.hpp"
|
||||
#include "MecanumPosControl/MecanumPosControl.hpp"
|
||||
|
||||
class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
|
||||
public px4::ScheduledWorkItem
|
||||
@ -91,9 +92,9 @@ private:
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode).
|
||||
* @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode).
|
||||
*/
|
||||
void generateSteeringSetpoint();
|
||||
void generateSteeringAndThrottleSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
|
||||
@ -126,9 +127,10 @@ private:
|
||||
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
|
||||
|
||||
// Class instances
|
||||
MecanumRateControl _mecanum_rate_control{this};
|
||||
MecanumAttControl _mecanum_att_control{this};
|
||||
MecanumPosVelControl _mecanum_pos_vel_control{this};
|
||||
MecanumRateControl _mecanum_rate_control{this};
|
||||
MecanumAttControl _mecanum_att_control{this};
|
||||
MecanumVelControl _mecanum_vel_control{this};
|
||||
MecanumPosControl _mecanum_pos_control{this};
|
||||
|
||||
// Variables
|
||||
hrt_abstime _timestamp{0};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user