vtol_att_control: occasional tailsitter forward transition failure issue solved

Tailsitter VTOLs very occasionally gets stuck with zero roll and pitch angle in multicopter mode after
a forward transition command is issued.
This very rare behavior is triggered by the following events:
1> a forward transition is triggered either in auto or manual mode.
2> in the vtol_att_control main loop, if multicopter and fixed wing attitude setpoints are not updated, transition state is not updated
3> the commander changes the vehicle status to transition mode.
4> multicopter pos controller initiated  Transition flight task. This results in zero roll and pitch setpoint due to zero acceleration setpoint
5> now vtol_att_control executes and updates the transition state. Specifically, _q_trans_start and _q_trans_sp are set with zero roll and pitch sp
6> tilt is evaluated to be NaN, despite _q_trans_sp being normalized. This happens for 25% of all yaw angles when using float datatype. This can be
   verified using the matrix library
7> once tilt is evaluated to be NaN, _q_trans_sp is never updated again and is stuck in this state for ever.

This has been fixed by constraining the cos(tilt) within +1 to -1 range
Further, _q_trans_start and _q_trans_sp are immedietly initialized after a transition event is triggered.

Author: nidhishraj

src/modules/vtol_att_control/tailsitter.cpp

@@ -225,8 +225,11 @@ void Tailsitter::update_transition_state()
 	_q_trans_sp.normalize();
 
 	// tilt angle (zero if vehicle nose points up (hover))
-	const float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) *
-				 _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3));
+	float cos_tilt = _q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) *
+			 _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3);
+	cos_tilt = cos_tilt >  1.0f ?  1.0f : cos_tilt;
+	cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt;
+	const float tilt = acosf(cos_tilt);
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -424,7 +424,7 @@ VtolAttitudeControl::Run()
 
 			_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
 
-			if (mc_att_sp_updated || fw_att_sp_updated) {
+			if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
 				_v_att_sp_pub.publish(_v_att_sp);
 			}

src/modules/vtol_att_control/vtol_type.h

@@ -190,6 +190,8 @@ class VtolType
 
 	mode get_mode() {return _vtol_mode;}
 
+	bool was_in_trans_mode() {return _flag_was_in_trans_mode;}
+
 	virtual void parameters_update() = 0;
 
 protected: