fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 13:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Replace rate controller with RateControlLibrary

Browse Source 
This commit makes the fw attitude controller take share the rate controller as a library with the mc_rate_control module
This commit is contained in:
Jaeyoung-Lim
2022-07-13 17:33:39 +02:00
committed by Silvan Fuhrer
parent 44a18acd51
commit f2877ce585
18 changed files with 70 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/CMakeLists.txt
+2
View File
@@ -30,6 +30,7 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__fw_att_control
MAIN fw_att_control
@@ -44,5 +45,6 @@ px4_add_module(
ecl_yaw_controller.cpp
DEPENDS
px4_work_queue
RateControl
SlewRate
)
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+36 -33
View File
@@ -34,6 +34,8 @@
#include "FixedwingAttitudeControl.hpp"
using namespace time_literals;
using namespace matrix;
using math::constrain;
using math::interpolate;
using math::radians;
@@ -81,23 +83,9 @@ FixedwingAttitudeControl::parameters_update()
{
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
_pitch_ctrl.set_k_p(_param_fw_pr_p.get());
_pitch_ctrl.set_k_i(_param_fw_pr_i.get());
_pitch_ctrl.set_k_ff(_param_fw_pr_ff.get());
_pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get());
/* roll control parameters */
_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
_roll_ctrl.set_k_p(_param_fw_rr_p.get());
_roll_ctrl.set_k_i(_param_fw_rr_i.get());
_roll_ctrl.set_k_ff(_param_fw_rr_ff.get());
_roll_ctrl.set_integrator_max(_param_fw_rr_imax.get());
/* yaw control parameters */
_yaw_ctrl.set_k_p(_param_fw_yr_p.get());
_yaw_ctrl.set_k_i(_param_fw_yr_i.get());
_yaw_ctrl.set_k_ff(_param_fw_yr_ff.get());
_yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get());
/* wheel control parameters */
_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
@@ -106,6 +94,19 @@ FixedwingAttitudeControl::parameters_update()
_wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
_wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get());
const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
const Vector3f rate_d = Vector3f(0.0f, 0.0f, 0.0f);
_rate_control.setGains(rate_p, rate_i, rate_d);
_rate_control.setIntegratorLimit(
Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
_rate_control.setFeedForwardGain(
Vector3f(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()));
return PX4_OK;
}
@@ -312,6 +313,11 @@ void FixedwingAttitudeControl::Run()
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];
const Vector3f rates(rollspeed, pitchspeed, yawspeed);
vehicle_angular_acceleration_s angular_acceleration{};
_vehicle_angular_acceleration_sub.copy(&angular_acceleration);
const Vector3f angular_accel{angular_acceleration.xyz};
if (_vehicle_status.is_vtol_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
@@ -531,9 +537,8 @@ void FixedwingAttitudeControl::Run()
}
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
Vector3f rates_setpoint = Vector3f(_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(),
_yaw_ctrl.get_desired_rate());
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
@@ -547,11 +552,13 @@ void FixedwingAttitudeControl::Run()
&& ((now - pid_autotune.timestamp) < 1_s)) {
bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp);
rates_setpoint = rates_setpoint + bodyrate_ff;
}
}
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
float roll_u = att_control(0) * _airspeed_scaling * _airspeed_scaling;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
@@ -559,7 +566,7 @@ void FixedwingAttitudeControl::Run()
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
@@ -576,7 +583,7 @@ void FixedwingAttitudeControl::Run()
yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get();
} else {
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
yaw_u = att_control(2) * _airspeed_scaling * _airspeed_scaling;
}
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
@@ -587,7 +594,7 @@ void FixedwingAttitudeControl::Run()
}
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
// _yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
@@ -626,19 +633,15 @@ void FixedwingAttitudeControl::Run()
} else {
vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
const Vector3f rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch :
trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control(
0) + trim_roll : trim_roll;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(att_control(1))) ? att_control(
1) + trim_pitch : trim_pitch;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(att_control(2))) ? att_control(
2) + trim_yaw : trim_yaw;
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
_rates_sp.thrust_body[0] : 0.0f;
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+7
View File
@@ -31,6 +31,10 @@
*
****************************************************************************/
#pragma once
#include <lib/rate_control/rate_control.hpp>
#include <drivers/drv_hrt.h>
#include "ecl_pitch_controller.h"
#include "ecl_roll_controller.h"
@@ -61,6 +65,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -120,6 +125,7 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
@@ -235,6 +241,7 @@ private:
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl;
RateControl _rate_control; ///< class for rate control calculations
/**
* @brief Update flap control setting
src/modules/fw_att_control/ecl_controller.h
-2
View File
@@ -80,8 +80,6 @@ public:
virtual ~ECL_Controller() = default;
virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) = 0;
virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
/* Setters */
void set_time_constant(float time_constant);
src/modules/fw_att_control/ecl_pitch_controller.cpp
+5 -60
View File
@@ -58,66 +58,11 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
float euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = cosf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
return _rate_setpoint;
}
float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}
src/modules/fw_att_control/ecl_pitch_controller.h
-2
View File
@@ -61,8 +61,6 @@ public:
~ECL_PitchController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
/* Additional Setters */
void set_max_rate_pos(float max_rate_pos)
src/modules/fw_att_control/ecl_roll_controller.cpp
+4 -58
View File
@@ -56,64 +56,10 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = roll_error / _tc;
float euler_rate_setpoint = roll_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_x_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}
src/modules/fw_att_control/ecl_roll_controller.h
-2
View File
@@ -59,8 +59,6 @@ public:
~ECL_RollController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
};
#endif // ECL_ROLL_CONTROLLER_H
src/modules/fw_att_control/ecl_wheel_controller.h
+2 -2
View File
@@ -60,9 +60,9 @@ public:
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data);
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override { (void)ctl_data; return 0; }
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; }
};
#endif // ECL_HEADING_CONTROLLER_H
src/modules/fw_att_control/ecl_yaw_controller.cpp
+6 -61
View File
@@ -82,8 +82,12 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
float euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = -sinf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * euler_rate_setpoint;
}
if (!PX4_ISFINITE(_rate_setpoint)) {
@@ -93,62 +97,3 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}
src/modules/fw_att_control/ecl_yaw_controller.h
-2
View File
@@ -59,8 +59,6 @@ public:
~ECL_YawController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
protected:
float _max_rate{0.0f};
src/modules/mc_rate_control/CMakeLists.txt
-2
View File
@@ -31,8 +31,6 @@
#
############################################################################
add_subdirectory(RateControl)
px4_add_module(
MODULE modules__mc_rate_control
MAIN mc_rate_control
src/modules/mc_rate_control/MulticopterRateControl.hpp
+1 -2
View File
@@ -33,8 +33,7 @@
#pragma once
#include <RateControl.hpp>
#include <lib/rate_control/rate_control.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
src/modules/mc_rate_control/RateControl/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2019 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(RateControl
RateControl.cpp
RateControl.hpp
)
target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(RateControl PRIVATE mathlib)
px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl)
src/modules/mc_rate_control/RateControl/RateControl.cpp
-111
View File
@@ -1,111 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
/**
* @file RateControl.cpp
*/
#include <RateControl.hpp>
#include <px4_platform_common/defines.h>
using namespace matrix;
void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
_gain_p = P;
_gain_i = I;
_gain_d = D;
}
void RateControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
const Vector<bool, 3> &saturation_negative)
{
_control_allocator_saturation_positive = saturation_positive;
_control_allocator_saturation_negative = saturation_negative;
}
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
const float dt, const bool landed)
{
// angular rates error
Vector3f rate_error = rate_sp - rate;
// PID control with feed forward
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
// update integral only if we are not landed
if (!landed) {
updateIntegral(rate_error, dt);
}
return torque;
}
void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
{
for (int i = 0; i < 3; i++) {
// prevent further positive control saturation
if (_control_allocator_saturation_positive(i)) {
rate_error(i) = math::min(rate_error(i), 0.f);
}
// prevent further negative control saturation
if (_control_allocator_saturation_negative(i)) {
rate_error(i) = math::max(rate_error(i), 0.f);
}
// I term factor: reduce the I gain with increasing rate error.
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
// change (noticeable in a bounce-back effect after a flip).
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
// and up to 200 deg error leads to <25% reduction of I.
float i_factor = rate_error(i) / math::radians(400.f);
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
// Perform the integration using a first order method
float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;
// do not propagate the result if out of range or invalid
if (PX4_ISFINITE(rate_i)) {
_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
}
}
}
void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
{
rate_ctrl_status.rollspeed_integ = _rate_int(0);
rate_ctrl_status.pitchspeed_integ = _rate_int(1);
rate_ctrl_status.yawspeed_integ = _rate_int(2);
}
src/modules/mc_rate_control/RateControl/RateControl.hpp
-119
View File
@@ -1,119 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
/**
* @file RateControl.hpp
*
* PID 3 axis angular rate / angular velocity control.
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <uORB/topics/rate_ctrl_status.h>
class RateControl
{
public:
RateControl() = default;
~RateControl() = default;
/**
* Set the rate control gains
* @param P 3D vector of proportional gains for body x,y,z axis
* @param I 3D vector of integral gains
* @param D 3D vector of derivative gains
*/
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
/**
* Set the mximum absolute value of the integrator for all axes
* @param integrator_limit limit value for all axes x, y, z
*/
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
/**
* Set direct rate to torque feed forward gain
* @see _gain_ff
* @param FF 3D vector of feed forward gains for body x,y,z axis
*/
void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
/**
* Set saturation status
* @param control saturation vector from control allocator
*/
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative);
/**
* Run one control loop cycle calculation
* @param rate estimation of the current vehicle angular rate
* @param rate_sp desired vehicle angular rate setpoint
* @param dt desired vehicle angular rate setpoint
* @return [-1,1] normalized torque vector to apply to the vehicle
*/
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,
const matrix::Vector3f &angular_accel, const float dt, const bool landed);
/**
* Set the integral term to 0 to prevent windup
* @see _rate_int
*/
void resetIntegral() { _rate_int.zero(); }
/**
* Get status message of controller for logging/debugging
* @param rate_ctrl_status status message to fill with internal states
*/
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
private:
void updateIntegral(matrix::Vector3f &rate_error, const float dt);
// Gains
matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
matrix::Vector3f _gain_i; ///< rate control integral gain
matrix::Vector3f _gain_d; ///< rate control derivative gain
matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
// States
matrix::Vector3f _rate_int; ///< integral term of the rate controller
// Feedback from control allocation
matrix::Vector<bool, 3> _control_allocator_saturation_negative;
matrix::Vector<bool, 3> _control_allocator_saturation_positive;
};
src/modules/mc_rate_control/RateControl/RateControlTest.cpp
-44
View File
@@ -1,44 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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 <gtest/gtest.h>
#include <RateControl.hpp>
using namespace matrix;
TEST(RateControlTest, AllZeroCase)
{
RateControl rate_control;
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
EXPECT_EQ(torque, Vector3f());
}