mc_attitude_control: move attitude setpoint pulling to right before u…

…sage Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Feb 6, 2024 · 982c998 · 982c998
1 parent e5cfbbb
commit 982c998
Showing 1 changed file with 30 additions and 29 deletions.
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -226,34 +226,6 @@ MulticopterAttitudeControl::Run()
 
 		const Quatf q{v_att.q};
 
-		// Check for new attitude setpoint
-		if (_vehicle_attitude_setpoint_sub.updated()) {
-			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
-
-			if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
-			    && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
-
-				_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
-				_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
-				_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
-			}
-		}
-
-		// Check for a heading reset
-		if (_quat_reset_counter != v_att.quat_reset_counter) {
-			const Quatf delta_q_reset(v_att.delta_q_reset);
-
-			// for stabilized attitude generation only extract the heading change from the delta quaternion
-			_man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi());
-
-			if (v_att.timestamp > _last_attitude_setpoint) {
-				// adapt existing attitude setpoint unless it was generated after the current attitude estimate
-				_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
-			}
-
-			_quat_reset_counter = v_att.quat_reset_counter;
-		}
-
 		/* check for updates in other topics */
 		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
 		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
@@ -295,7 +267,8 @@ MulticopterAttitudeControl::Run()
 		// vehicle is a tailsitter in transition mode
 		const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
 
-		bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
+		const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering
+					  || is_tailsitter_transition);
 
 		if (run_att_ctrl) {
 
@@ -313,6 +286,34 @@ MulticopterAttitudeControl::Run()
 				_man_pitch_input_filter.reset(0.f);
 			}
 
+			// Check for new attitude setpoint
+			if (_vehicle_attitude_setpoint_sub.updated()) {
+				vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+
+				if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
+				    && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
+
+					_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
+					_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
+					_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
+				}
+			}
+
+			// Check for a heading reset
+			if (_quat_reset_counter != v_att.quat_reset_counter) {
+				const Quatf delta_q_reset(v_att.delta_q_reset);
+
+				// for stabilized attitude generation only extract the heading change from the delta quaternion
+				_man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi());
+
+				if (v_att.timestamp > _last_attitude_setpoint) {
+					// adapt existing attitude setpoint unless it was generated after the current attitude estimate
+					_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
+				}
+
+				_quat_reset_counter = v_att.quat_reset_counter;
+			}
+
 			Vector3f rates_sp = _attitude_control.update(q);
 
 			const hrt_abstime now = hrt_absolute_time();