Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

mc_att_control: only adapt yaw rate limit on control mode change

Browse Source 
Put adaption into a method because it needs to be called when
the control mode or the parameter changes.
This commit is contained in:
Matthias Grob
2019-03-10 23:00:05 +01:00
parent 3375ae2c11
commit 463d5512d9
2 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
+3
View File
@@ -147,6 +147,9 @@ private:
*/
matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate);
/** lower yawspeed limit in auto modes because we expect yaw steps */
void adapt_auto_yaw_rate_limit();
AttitudeControl _attitude_control; /**< class for attitude control calculations */
int _v_att_sub{-1}; /**< vehicle attitude subscription */
src/modules/mc_att_control/mc_att_control_main.cpp
+11 -8
View File
@@ -149,6 +149,7 @@ MulticopterAttitudeControl::parameters_updated()
// angular rate limits
using math::radians;
_attitude_control.setRateLimit(Vector3f(radians(_roll_rate_max.get()), radians(_pitch_rate_max.get()), radians(_yaw_rate_max.get())));
adapt_auto_yaw_rate_limit();
// manual rate control acro mode rate limits
_acro_rate_max = Vector3f(radians(_acro_roll_max.get()), radians(_acro_pitch_max.get()), radians(_acro_yaw_max.get()));
@@ -194,6 +195,16 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
adapt_auto_yaw_rate_limit();
}
}
void MulticopterAttitudeControl::adapt_auto_yaw_rate_limit() {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get()));
} else {
_attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get()));
}
}
@@ -518,14 +529,6 @@ MulticopterAttitudeControl::control_attitude()
// physical thrust axis is the negative of body z axis
_thrust_sp = -_v_att_sp.thrust_body[2];
// lower yawspeed limit in auto modes because we expect yaw steps
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get()));
} else {
_attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get()));
}
_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
}