tailsitter: removed usage of hrt_elapsed and small cleanup

- do not call hrt_elapsed_time since it's expensive
- remove P2 front transition phase (was not even used)

Signed-off-by: Roman <bapstroman@gmail.com>

src/modules/vtol_att_control/tailsitter.cpp

@@ -45,7 +45,6 @@
 #define ARSP_YAW_CTRL_DISABLE 4.0f	// airspeed at which we stop controlling yaw during a front transition
 #define THROTTLE_TRANSITION_MAX 0.25f	// maximum added thrust above last value in transition
 #define PITCH_TRANSITION_FRONT_P1 -1.1f	// pitch angle to switch to TRANSITION_P2
-#define PITCH_TRANSITION_FRONT_P2 -1.2f	// pitch angle to switch to FW
 #define PITCH_TRANSITION_BACK -0.25f	// pitch angle to switch to MC
 
 Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
@@ -109,12 +108,6 @@ void Tailsitter::update_vtol_state()
 			_vtol_schedule.flight_mode = MC_MODE;
 			break;
 
-		case TRANSITION_FRONT_P2:
-			// NOT USED
-			// failsafe into multicopter mode
-			//_vtol_schedule.flight_mode = MC_MODE;
-			break;
-
 		case TRANSITION_BACK:
 
 			// check if we have reached pitch angle to switch to MC mode
@@ -143,20 +136,13 @@ void Tailsitter::update_vtol_state()
 			if ((_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed
 			     && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
 				_vtol_schedule.flight_mode = FW_MODE;
-				//_vtol_schedule.transition_start = hrt_absolute_time();
 			}
 
 			break;
 
-		case TRANSITION_FRONT_P2:
-
 		case TRANSITION_BACK:
 			// failsafe into fixed wing mode
 			_vtol_schedule.flight_mode = FW_MODE;
-
-			/*  **LATER***  if pitch is closer to mc (-45>)
-			*   go to transition P1
-			*/
 			break;
 		}
 	}
@@ -180,11 +166,6 @@ void Tailsitter::update_vtol_state()
 		_vtol_vehicle_status->vtol_in_trans_mode = true;
 		break;
 
-	case TRANSITION_FRONT_P2:
-		_vtol_mode = TRANSITION_TO_FW;
-		_vtol_vehicle_status->vtol_in_trans_mode = true;
-		break;
-
 	case TRANSITION_BACK:
 		_vtol_mode = TRANSITION_TO_MC;
 		_vtol_vehicle_status->vtol_in_trans_mode = true;
@@ -194,6 +175,8 @@ void Tailsitter::update_vtol_state()
 
 void Tailsitter::update_transition_state()
 {
+	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start);
+
 	if (!_flag_was_in_trans_mode) {
 		// save desired heading for transition and last thrust value
 		_yaw_transition = _v_att_sp->yaw_body;
@@ -206,9 +189,9 @@ void Tailsitter::update_transition_state()
 
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 
-		/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
+		// create time dependant pitch angle set point + 0.2 rad overlap over the switch value
 		_v_att_sp->pitch_body = _pitch_transition_start	- (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
-					(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params->front_trans_duration * 1000000.0f));
+					time_since_trans_start / (_params->front_trans_duration * 1e6f));
 		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,
 							_pitch_transition_start);
 
@@ -225,43 +208,6 @@ void Tailsitter::update_transition_state()
 		_mc_roll_weight = 1.0f;
 		_mc_pitch_weight = 1.0f;
 
-	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
-		// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
-
-		/** no motor  switching */
-
-		if (flag_idle_mc) {
-			set_idle_fw();
-			flag_idle_mc = false;
-		}
-
-		/** create time dependant pitch angle set point  + 0.2 rad overlap over the switch value*/
-		if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
-			_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
-						(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(
-							 &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
-
-			if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
-				_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;
-			}
-
-		}
-
-		_v_att_sp->thrust = _thrust_transition;
-
-		/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
-
-		//_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
-		//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
-
-
-		_mc_roll_weight = 0.0f;
-		_mc_pitch_weight = 0.0f;
-
-		/** keep yaw disabled */
-		_mc_yaw_weight = 0.0f;
-
-
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 
 		if (!flag_idle_mc) {
@@ -269,22 +215,20 @@ void Tailsitter::update_transition_state()
 			flag_idle_mc = true;
 		}
 
-		/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/
+		// create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
 		_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
-					(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params->back_trans_duration * 1000000.0f);
+					time_since_trans_start / (_params->back_trans_duration * 1e6f);
 		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f);
 
 
 		_v_att_sp->thrust = _mc_virtual_att_sp->thrust;
 
-		/** keep yaw disabled */
+		// keep yaw disabled
 		_mc_yaw_weight = 0.0f;
 
-		/** smoothly move control weight to MC */
+		// smoothly move control weight to MC
-		_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
+		_mc_roll_weight = 1.0f * time_since_trans_start / (_params->back_trans_duration * 1e6f);
-				  (_params->back_trans_duration * 1000000.0f);
+		_mc_pitch_weight = 1.0f * time_since_trans_start / (_params->back_trans_duration * 1e6f);
-		_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
-				   (_params->back_trans_duration * 1000000.0f);
 
 	}
 

src/modules/vtol_att_control/tailsitter.h

@@ -74,7 +74,6 @@ private:
 	enum vtol_mode {
 		MC_MODE = 0,			/**< vtol is in multicopter mode */
 		TRANSITION_FRONT_P1,	/**< vtol is in front transition part 1 mode */
-		TRANSITION_FRONT_P2,	/**< vtol is in front transition part 2 mode */
 		TRANSITION_BACK,		/**< vtol is in back transition mode */
 		FW_MODE					/**< vtol is in fixed wing mode */
 	};