fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ackermann: separate actuator control

Browse Source
This commit is contained in:
chfriedrich98 2025-04-21 12:35:10 +02:00 committed by chfriedrich98
parent ea94bc11eb
commit 45540455fe
6 changed files with 265 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp Normal file
View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* 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 "AckermannActControl.hpp"
using namespace time_literals;
AckermannActControl::AckermannActControl(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
}
void AckermannActControl::updateParams()
{
ModuleParams::updateParams();
if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) {
_servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get());
}
if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
_motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
}
}
void AckermannActControl::updateActControl()
{
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
generateActuatorSetpoint();
}
void AckermannActControl::generateActuatorSetpoint()
{
// Motor control
rover_throttle_setpoint_s rover_throttle_setpoint{};
_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
actuator_motors_s actuator_motors_sub{};
_actuator_motors_sub.copy(&actuator_motors_sub);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
rover_throttle_setpoint.throttle_body_x, actuator_motors_sub.control[0], _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
// Servo control
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
actuator_servos_s actuator_servos_sub{};
_actuator_servos_sub.copy(&actuator_servos_sub);
if (_param_ra_str_rate_limit.get() > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf(
rover_steering_setpoint.normalized_steering_angle -
actuator_servos_sub.control[0])) {
_servo_setpoint.setForcedValue(actuator_servos_sub.control[0]);
}
_servo_setpoint.update(rover_steering_setpoint.normalized_steering_angle, _dt);
} else {
_servo_setpoint.setForcedValue(rover_steering_setpoint.normalized_steering_angle);
}
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = _servo_setpoint.getState();
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}

111
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp Normal file
View File

@ -0,0 +1,111 @@
/****************************************************************************
*
* 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>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
/**
* @brief Class for ackermann actuator control.
*/
class AckermannActControl : public ModuleParams
{
public:
/**
* @brief Constructor for AckermannActControl.
* @param parent The parent ModuleParams object.
*/
AckermannActControl(ModuleParams *parent);
~AckermannActControl() = default;
/**
* @brief Update actuator controller.
*/
void updateActControl();
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
private:
/**
* @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void generateActuatorSetpoint();
// uORB subscriptions
uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
// uORB publications
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
// Variables
hrt_abstime _timestamp{0};
float _dt{0.f};
// Controllers
SlewRate<float> _servo_setpoint{0.f};
SlewRate<float> _motor_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};

38
src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# 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(AckermannActControl
AckermannActControl.cpp
)
target_include_directories(AckermannActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

2
src/modules/rover_ackermann/CMakeLists.txt
View File

@ -31,6 +31,7 @@
#
############################################################################
add_subdirectory(AckermannActControl)
add_subdirectory(AckermannRateControl)
add_subdirectory(AckermannAttControl)
add_subdirectory(AckermannVelControl)
@ -43,6 +44,7 @@ px4_add_module(
RoverAckermann.cpp
RoverAckermann.hpp
DEPENDS
AckermannActControl
AckermannRateControl
AckermannAttControl
AckermannVelControl

80
src/modules/rover_ackermann/RoverAckermann.cpp
View File

@ -53,14 +53,6 @@ bool RoverAckermann::init()
void RoverAckermann::updateParams()
{
ModuleParams::updateParams();
if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) {
_servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get());
}
if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
_motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
}
}
void RoverAckermann::Run()
@ -69,15 +61,6 @@ void RoverAckermann::Run()
updateParams();
}
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
_ackermann_pos_control.updatePosControl();
_ackermann_vel_control.updateVelControl();
_ackermann_att_control.updateAttControl();
_ackermann_rate_control.updateRateControl();
if (_vehicle_control_mode_sub.updated()) {
_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
}
@ -90,8 +73,12 @@ void RoverAckermann::Run()
generateSteeringAndThrottleSetpoint();
}
generateActuatorSetpoint();
_ackermann_pos_control.updatePosControl();
_ackermann_vel_control.updateVelControl();
_ackermann_att_control.updateAttControl();
_ackermann_rate_control.updateRateControl();
_ackermann_act_control.updateActControl();
}
void RoverAckermann::generateSteeringAndThrottleSetpoint()
@ -100,70 +87,17 @@ void RoverAckermann::generateSteeringAndThrottleSetpoint()
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.timestamp = hrt_absolute_time();
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
}
void RoverAckermann::generateActuatorSetpoint()
{
if (_rover_throttle_setpoint_sub.updated()) {
_rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint);
}
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
_current_motor_setpoint = actuator_motors.control[0];
}
if (_vehicle_control_mode.flag_armed) {
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
_rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(),
_param_ro_max_thr_speed.get(), _dt);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
if (_rover_steering_setpoint_sub.updated()) {
_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
}
if (_actuator_servos_sub.updated()) {
actuator_servos_s actuator_servos{};
_actuator_servos_sub.copy(&actuator_servos);
_current_servo_setpoint = actuator_servos.control[0];
}
if (_param_ra_str_rate_limit.get() > FLT_EPSILON
&& _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf(
_rover_steering_setpoint.normalized_steering_angle -
_current_servo_setpoint)) {
_servo_setpoint.setForcedValue(_current_servo_setpoint);
}
_servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt);
} else {
_servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle);
}
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = _servo_setpoint.getState();
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}
int RoverAckermann::task_spawn(int argc, char *argv[])
{
RoverAckermann *instance = new RoverAckermann();

42
src/modules/rover_ackermann/RoverAckermann.hpp
View File

@ -40,24 +40,17 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// Libraries
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <lib/rover_control/RoverControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
// Local includes
#include "AckermannActControl/AckermannActControl.hpp"
#include "AckermannRateControl/AckermannRateControl.hpp"
#include "AckermannAttControl/AckermannAttControl.hpp"
#include "AckermannVelControl/AckermannVelControl.hpp"
@ -98,52 +91,21 @@ private:
*/
void generateSteeringAndThrottleSetpoint();
/**
* @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void generateActuatorSetpoint();
// uORB subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
vehicle_control_mode_s _vehicle_control_mode{};
rover_steering_setpoint_s _rover_steering_setpoint{};
rover_throttle_setpoint_s _rover_throttle_setpoint{};
// uORB publications
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
// Class instances
AckermannActControl _ackermann_act_control{this};
AckermannRateControl _ackermann_rate_control{this};
AckermannAttControl _ackermann_att_control{this};
AckermannVelControl _ackermann_vel_control{this};
AckermannPosControl _ackermann_pos_control{this};
// Variables
hrt_abstime _timestamp{0};
float _dt{0.f};
float _current_servo_setpoint{0.f};
float _current_motor_setpoint{0.f};
// Controllers
SlewRate<float> _servo_setpoint{0.f};
SlewRate<float> _motor_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};