mc_pos_control: switch order of setpoint amendment

to make sure the takeoff limitation is always done last.
PX4 · Mar 9, 2020 · aba5a41 · aba5a41
1 parent eab8829
commit aba5a41
Showing 1 changed file with 10 additions and 9 deletions.
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -577,6 +577,16 @@ MulticopterPositionControl::Run()
 			// check if all local states are valid and map accordingly
 			set_vehicle_states(setpoint.vz);
 
+			// limit tilt during takeoff ramupup
+			if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
+				constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
+			}
+
+			// limit altitude only if local position is valid
+			if (PX4_ISFINITE(_states.position(2))) {
+				limit_altitude(setpoint);
+			}
+
 			// handle smooth takeoff
 			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
 						    constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
@@ -605,15 +615,6 @@ MulticopterPositionControl::Run()
 				_hover_thrust_estimator.update(_dt);
 			}
 
-			if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
-				constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
-			}
-
-			// limit altitude only if local position is valid
-			if (PX4_ISFINITE(_states.position(2))) {
-				limit_altitude(setpoint);
-			}
-
 			// Run position control
 			_control.setState(_states);
 			_control.setConstraints(constraints);