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

differential: seperate actuator control

Browse Source
This commit is contained in:
chfriedrich98 2025-05-06 09:35:42 +02:00 committed by chfriedrich98
parent b26dd4d3f3
commit ac80958cc5
6 changed files with 292 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,6 +31,7 @@
#
############################################################################
add_subdirectory(DifferentialActControl)
add_subdirectory(DifferentialRateControl)
add_subdirectory(DifferentialAttControl)
add_subdirectory(DifferentialVelControl)
@ -43,6 +44,7 @@ px4_add_module(
RoverDifferential.cpp
RoverDifferential.hpp
DEPENDS
DifferentialActControl
DifferentialRateControl
DifferentialAttControl
DifferentialVelControl

38
src/modules/rover_differential/DifferentialActControl/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(DifferentialActControl
DifferentialActControl.cpp
)
target_include_directories(DifferentialActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

125
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp Normal file
View File

@ -0,0 +1,125 @@
/****************************************************************************
*
* 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 "DifferentialActControl.hpp"
using namespace time_literals;
DifferentialActControl::DifferentialActControl(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
}
void DifferentialActControl::updateParams()
{
ModuleParams::updateParams();
if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
_adjusted_throttle_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
}
}
void DifferentialActControl::updateActControl()
{
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// Motor control
if (_rover_throttle_setpoint_sub.updated()) {
rover_throttle_setpoint_s rover_throttle_setpoint{};
_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
_throttle_setpoint = rover_throttle_setpoint.throttle_body_x;
}
if (_rover_steering_setpoint_sub.updated()) {
rover_steering_setpoint_s rover_steering_setpoint{};
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
_speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff;
}
if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
actuator_motors_s actuator_motors_sub{};
_actuator_motors_sub.copy(&actuator_motors_sub);
const float current_throttle = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f;
const float adjusted_throttle_setpoint = RoverControl::throttleControl(_adjusted_throttle_setpoint,
_throttle_setpoint, current_throttle, _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
computeInverseKinematics(adjusted_throttle_setpoint, _speed_diff_setpoint).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
}
Vector2f DifferentialActControl::computeInverseKinematics(float throttle, const float speed_diff_normalized)
{
float max_motor_command = fabsf(throttle) + fabsf(speed_diff_normalized);
if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1
float excess = fabsf(max_motor_command - 1.0f);
throttle -= sign(throttle) * excess;
}
// Calculate the left and right wheel speeds
return Vector2f(throttle - speed_diff_normalized,
throttle + speed_diff_normalized);
}
void DifferentialActControl::manualManualMode()
{
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll;
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
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 DifferentialActControl::stopVehicle()
{
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = 0.f;
actuator_motors.control[1] = 0.f;
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}

121
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
*
* 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/topics/actuator_motors.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
/**
* @brief Class for differential actuator control.
*/
class DifferentialActControl : public ModuleParams
{
public:
/**
* @brief Constructor for DifferentialActControl.
* @param parent The parent ModuleParams object.
*/
DifferentialActControl(ModuleParams *parent);
~DifferentialActControl() = default;
/**
* @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void updateActControl();
/**
* @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
*/
void manualManualMode();
/**
* @brief Stop the vehicle by sending 0 commands to motors and servos.
*/
void stopVehicle();
protected:
/**
* @brief Update the parameters of the module.
*/
void updateParams() override;
private:
/**
* @brief Compute normalized motor commands based on normalized setpoints.
* @param throttle Normalized speed in body x direction [-1, 1].
* @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
* @return Motor speeds for the right and left motors [-1, 1].
*/
Vector2f computeInverseKinematics(float throttle, float speed_diff_normalized);
// uORB subscriptions
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::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
// uORB publications
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
// Variables
hrt_abstime _timestamp{0};
float _throttle_setpoint{NAN};
float _speed_diff_setpoint{NAN};
// Controllers
SlewRate<float> _adjusted_throttle_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(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
)
};

74
src/modules/rover_differential/RoverDifferential.cpp
View File

@ -39,8 +39,6 @@ RoverDifferential::RoverDifferential() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
_rover_throttle_setpoint_pub.advertise();
_rover_steering_setpoint_pub.advertise();
updateParams();
}
@ -53,10 +51,6 @@ bool RoverDifferential::init()
void RoverDifferential::updateParams()
{
ModuleParams::updateParams();
if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
_throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
}
}
void RoverDifferential::Run()
@ -65,10 +59,6 @@ void RoverDifferential::Run()
updateParams();
}
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
_differential_pos_control.updatePosControl();
_differential_vel_control.updateVelControl();
_differential_att_control.updateAttControl();
@ -83,74 +73,18 @@ void RoverDifferential::Run()
&& !_vehicle_control_mode.flag_control_rates_enabled;
if (full_manual_mode_enabled) { // Manual mode
generateSteeringAndThrottleSetpoint();
_differential_act_control.manualManualMode();
}
if (_vehicle_control_mode.flag_armed) {
generateActuatorSetpoint();
_differential_act_control.updateActControl();
} else {
_differential_act_control.stopVehicle();
}
}
void RoverDifferential::generateSteeringAndThrottleSetpoint()
{
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = _timestamp;
rover_steering_setpoint.normalized_speed_diff = 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.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
}
}
void RoverDifferential::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_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f;
}
if (_rover_steering_setpoint_sub.updated()) {
_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
}
const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint,
_rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(),
_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
computeInverseKinematics(throttle_body_x,
_rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized)
{
float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized);
if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1
float excess = fabsf(max_motor_command - 1.0f);
throttle_body_x -= sign(throttle_body_x) * excess;
}
// Calculate the left and right wheel speeds
return Vector2f(throttle_body_x - speed_diff_normalized,
throttle_body_x + speed_diff_normalized);
}
int RoverDifferential::task_spawn(int argc, char *argv[])
{
RoverDifferential *instance = new RoverDifferential();

57
src/modules/rover_differential/RoverDifferential.hpp
View File

@ -40,22 +40,13 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// Libraries
#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/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 "DifferentialActControl/DifferentialActControl.hpp"
#include "DifferentialRateControl/DifferentialRateControl.hpp"
#include "DifferentialAttControl/DifferentialAttControl.hpp"
#include "DifferentialVelControl/DifferentialVelControl.hpp"
@ -91,59 +82,15 @@ protected:
private:
void Run() override;
/**
* @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode).
*/
void generateSteeringAndThrottleSetpoint();
/**
* @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
*/
void generateActuatorSetpoint();
/**
* @brief Compute normalized motor commands based on normalized setpoints.
* @param throttle_body_x Normalized speed in body x direction [-1, 1].
* @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
* @return Motor speeds for the right and left motors [-1, 1].
*/
Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized);
// 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_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<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
DifferentialActControl _differential_act_control{this};
DifferentialRateControl _differential_rate_control{this};
DifferentialAttControl _differential_att_control{this};
DifferentialVelControl _differential_vel_control{this};
DifferentialPosControl _differential_pos_control{this};
// Variables
hrt_abstime _timestamp{0};
float _dt{0.f};
float _current_throttle_body_x{0.f};
// Controllers
SlewRate<float> _throttle_body_x_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(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
)
};