VTOL: in update_transition_state(): check that needed virtual attitude setpoints are recent

This is to prevent that that data from non-recent publications of the virtual
attitude setpoints are used in the transition code.
It removes the previous implementation where the update_transition_state()
method was only run when both mc and fw virtual att sp where recent, independetly
of whether both are used or not.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/standard.cpp

@@ -203,18 +203,29 @@ void Standard::update_vtol_state()
 
 void Standard::update_transition_state()
 {
+	const hrt_abstime now = hrt_absolute_time();
 	float mc_weight = 1.0f;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
+	float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
 
 	VtolType::update_transition_state();
 
-	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
+	// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
+		if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
 
 	} else {
+		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
+		if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
 	}

src/modules/vtol_att_control/tailsitter.cpp

@@ -188,7 +188,13 @@ void Tailsitter::update_vtol_state()
 
 void Tailsitter::update_transition_state()
 {
-	const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
+	const hrt_abstime now = hrt_absolute_time();
+	const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
+
+	// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
+	if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) {
+		return;
+	}
 
 	if (!_flag_was_in_trans_mode) {
 		_flag_was_in_trans_mode = true;

src/modules/vtol_att_control/tiltrotor.cpp

@@ -280,19 +280,31 @@ void Tiltrotor::update_transition_state()
 {
 	VtolType::update_transition_state();
 
+	const hrt_abstime now = hrt_absolute_time();
+
 	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
+		if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else {
+		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
+		if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
 	}
 
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
+	float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
 		// for the first part of the transition all rotors are enabled

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -322,11 +322,6 @@ VtolAttitudeControl::Run()
 		const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
 		const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
 
-		// for transition code to publish an attitude setpoint require both mc and fw virtual attitude setpoint to not contain data older than one second.
-		// this prevents either topic from being used with old data at the time when we switch into transition mode
-		const bool mc_and_fw_att_sp_are_recent = _mc_virtual_att_sp.timestamp > (now - 1_s)
-				&& _fw_virtual_att_sp.timestamp > (now - 1_s);
-
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();
 
@@ -336,7 +331,7 @@ VtolAttitudeControl::Run()
 			// vehicle is doing a transition to FW
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
 
-			if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
 			}
@@ -347,7 +342,7 @@ VtolAttitudeControl::Run()
 			// vehicle is doing a transition to MC
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
 
-			if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
 			}