mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 18:13:33 +08:00
mecanum: refactor code architecture
This commit is contained in:
chfriedrich98
committed by
chfriedrich98
chfriedrich98
parent
d602b569b1
commit
7c63468e8b
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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(MecanumAttControl
|
||||
MecanumAttControl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(MecanumAttControl PUBLIC PID)
|
||||
target_include_directories(MecanumAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,215 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "MecanumAttControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_rover_rate_setpoint_pub.advertise();
|
||||
_rover_throttle_setpoint_pub.advertise();
|
||||
_rover_attitude_setpoint_pub.advertise();
|
||||
_rover_attitude_status_pub.advertise();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void MecanumAttControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) {
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
}
|
||||
|
||||
_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
|
||||
_pid_yaw.setIntegralLimit(_max_yaw_rate);
|
||||
_pid_yaw.setOutputLimit(_max_yaw_rate);
|
||||
_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
|
||||
}
|
||||
|
||||
void MecanumAttControl::updateAttControl()
|
||||
{
|
||||
const hrt_abstime timestamp_prev = _timestamp;
|
||||
_timestamp = hrt_absolute_time();
|
||||
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
|
||||
|
||||
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);
|
||||
matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
|
||||
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
|
||||
generateAttitudeSetpoint();
|
||||
}
|
||||
|
||||
generateRateSetpoint();
|
||||
|
||||
} else { // Reset pid and slew rate when attitude control is not active
|
||||
_pid_yaw.resetIntegral();
|
||||
_adjusted_yaw_setpoint.setForcedValue(0.f);
|
||||
}
|
||||
|
||||
// Publish attitude controller status (logging only)
|
||||
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_pub.publish(rover_attitude_status);
|
||||
|
||||
}
|
||||
|
||||
void MecanumAttControl::generateAttitudeSetpoint()
|
||||
{
|
||||
const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
|
||||
&& !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
|
||||
|
||||
if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = _timestamp;
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
|
||||
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
|
||||
_stab_yaw_setpoint = NAN;
|
||||
_adjusted_yaw_setpoint.setForcedValue(0.f);
|
||||
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(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON
|
||||
|| fabsf(rover_throttle_setpoint.throttle_body_y) >
|
||||
FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
|
||||
if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
|
||||
_stab_yaw_setpoint = _vehicle_yaw;
|
||||
}
|
||||
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = _timestamp;
|
||||
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
|
||||
} else { // Reset yaw control and yaw rate setpoint
|
||||
_stab_yaw_setpoint = NAN;
|
||||
_adjusted_yaw_setpoint.setForcedValue(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 if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
|
||||
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_att_control = _offboard_control_mode.attitude;
|
||||
|
||||
if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) {
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = _timestamp;
|
||||
rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MecanumAttControl::generateRateSetpoint()
|
||||
{
|
||||
if (_rover_attitude_setpoint_sub.updated()) {
|
||||
_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
|
||||
}
|
||||
|
||||
if (_rover_rate_setpoint_sub.updated()) {
|
||||
_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
|
||||
}
|
||||
|
||||
// Check if a new rate setpoint was already published from somewhere else
|
||||
if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
|
||||
&& _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
|
||||
_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
|
||||
|
||||
_last_rate_setpoint_update = _timestamp;
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = _timestamp;
|
||||
rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
}
|
||||
|
||||
bool MecanumAttControl::runSanityChecks()
|
||||
{
|
||||
bool ret = true;
|
||||
|
||||
if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (_param_ro_yaw_p.get() < FLT_EPSILON) {
|
||||
ret = false;
|
||||
|
||||
if (_prev_param_check_passed) {
|
||||
events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
|
||||
"Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
|
||||
}
|
||||
}
|
||||
|
||||
_prev_param_check_passed = ret;
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <lib/slew_rate/SlewRateYaw.hpp>
|
||||
#include <math.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
// 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/vehicle_control_mode.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/rover_attitude_status.h>
|
||||
#include <uORB/topics/rover_attitude_setpoint.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
|
||||
/**
|
||||
* @brief Class for mecanum attitude control.
|
||||
*/
|
||||
class MecanumAttControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for MecanumAttControl.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
MecanumAttControl(ModuleParams *parent);
|
||||
~MecanumAttControl() = default;
|
||||
|
||||
/**
|
||||
* @brief Update attitude controller.
|
||||
*/
|
||||
void updateAttControl();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode)
|
||||
* or trajectorySetpoint (Offboard attitude control).
|
||||
*/
|
||||
void generateAttitudeSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
|
||||
*/
|
||||
void generateRateSetpoint();
|
||||
|
||||
/**
|
||||
* @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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
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 _actuator_motors_sub{ORB_ID(actuator_motors)};
|
||||
uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
|
||||
uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
rover_attitude_setpoint_s _rover_attitude_setpoint{};
|
||||
rover_rate_setpoint_s _rover_rate_setpoint{};
|
||||
offboard_control_mode_s _offboard_control_mode{};
|
||||
|
||||
// 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_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
|
||||
|
||||
// Variables
|
||||
hrt_abstime _timestamp{0};
|
||||
hrt_abstime _last_rate_setpoint_update{0};
|
||||
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]
|
||||
bool _prev_param_check_passed{true};
|
||||
|
||||
// Controllers
|
||||
PID _pid_yaw;
|
||||
SlewRateYaw<float> _adjusted_yaw_setpoint;
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(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::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
|
||||
)
|
||||
};
|
||||
Reference in New Issue
Block a user