fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 17:50: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/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;