fw_att_control move angular rate limits to body angular rates (#9023)

* ecl: attitude_fw update - move angular rate limits to body angular rates - handle rattitude/acro vs attitude mode rate limits
PX4 · Mar 9, 2018 · 7473027 · 7473027
1 parent 2ce8b6a
commit 7473027
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 5 deletions.
diff --git a/src/lib/ecl b/src/lib/ecl
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -199,23 +199,19 @@ FixedwingAttitudeControl::parameters_update()
 	_pitch_ctrl.set_k_i(_parameters.p_i);
 	_pitch_ctrl.set_k_ff(_parameters.p_ff);
 	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
-	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
-	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.r_tc);
 	_roll_ctrl.set_k_p(_parameters.r_p);
 	_roll_ctrl.set_k_i(_parameters.r_i);
 	_roll_ctrl.set_k_ff(_parameters.r_ff);
 	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
-	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 
 	/* yaw control parameters */
 	_yaw_ctrl.set_k_p(_parameters.y_p);
 	_yaw_ctrl.set_k_i(_parameters.y_i);
 	_yaw_ctrl.set_k_ff(_parameters.y_ff);
 	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
-	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	/* wheel control parameters */
 	_wheel_ctrl.set_k_p(_parameters.w_p);
@@ -638,6 +634,24 @@ void FixedwingAttitudeControl::run()
 				control_input.groundspeed = groundspeed;
 				control_input.groundspeed_scaler = groundspeed_scaler;
 
+				/* reset body angular rate limits on mode change */
+				if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
+					if (_vcontrol_mode.flag_control_attitude_enabled) {
+						_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
+						_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
+						_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
+						_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
+
+					} else {
+						_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
+						_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
+						_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
+						_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
+					}
+				}
+
+				_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
+
 				/* Run attitude controllers */
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
 					if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -137,6 +137,8 @@ class FixedwingAttitudeControl final : public control::SuperBlock, public Module
 
 	float _battery_scale{1.0f};
 
+	bool _flag_control_attitude_enabled_last{false};
+
 	struct {
 		float p_tc;
 		float p_p;