vtol_att_control: handle front transition timeout for all type

- moved front transition timout logic to base class so that it's available for all vtol types - cleaned up variable usage Signed-off-by: Roman <bapstroman@gmail.com>
PX4 · Apr 19, 2018 · 352565f · 352565f
1 parent b066ef3
commit 352565f
Show file tree

Hide file tree

Showing 8 changed files with 137 additions and 144 deletions.
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
@@ -47,19 +47,9 @@
 #include <float.h>
 
 Standard::Standard(VtolAttitudeControl *attc) :
-	VtolType(attc),
-	_pusher_throttle(0.0f),
-	_reverse_output(0.0f),
-	_airspeed_trans_blend_margin(0.0f)
+	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = MC_MODE;
-	_vtol_schedule.transition_start = 0;
-	_pusher_active = false;
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-	_mc_throttle_weight = 1.0f;
+	_flight_mode = MC_MODE;
 
 	_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
 	_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
@@ -117,86 +107,86 @@ void Standard::update_vtol_state()
 	 * For the back transition the pusher motor is immediately stopped and rotors reactivated.
 	 */
 
+	VtolType::update_vtol_state();
+
 	float mc_weight = _mc_roll_weight;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (!_attc->is_fixed_wing_requested()) {
 
 		// the transition to fw mode switch is off
-		if (_vtol_schedule.flight_mode == MC_MODE) {
+		if (_flight_mode == MC_MODE) {
 			// in mc mode
-			_vtol_schedule.flight_mode = MC_MODE;
+			_flight_mode = MC_MODE;
 			mc_weight = 1.0f;
 			_pusher_throttle = 0.0f;
 			_reverse_output = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+		} else if (_flight_mode == FW_MODE) {
 			// transition to mc mode
 			if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
 				// Failsafe event, engage mc motors immediately
-				_vtol_schedule.flight_mode = MC_MODE;
+				_flight_mode = MC_MODE;
 				_pusher_throttle = 0.0f;
 				_reverse_output = 0.0f;
 
 
 			} else {
 				// Regular backtransition
-				_vtol_schedule.flight_mode = TRANSITION_TO_MC;
-				_vtol_schedule.transition_start = hrt_absolute_time();
+				_flight_mode = TRANSITION_TO_MC;
+				_transition_start_time = hrt_absolute_time();
 				_reverse_output = _params_standard.reverse_output;
 
 			}
 
-		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+		} else if (_flight_mode == TRANSITION_TO_FW) {
 			// failsafe back to mc mode
-			_vtol_schedule.flight_mode = MC_MODE;
+			_flight_mode = MC_MODE;
 			mc_weight = 1.0f;
 			_pusher_throttle = 0.0f;
 			_reverse_output = 0.0f;
 
 
-		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+		} else if (_flight_mode == TRANSITION_TO_MC) {
 			// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
 
 			const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
 			const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
 
 			float x_vel = vel(0);
 
-			if (time_since_trans_start > _params->back_trans_duration ||
+			if (get_time_since_trans_start() > _params->back_trans_duration ||
 			    (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) {
-				_vtol_schedule.flight_mode = MC_MODE;
+				_flight_mode = MC_MODE;
 			}
 
 		}
 
 	} else {
 		// the transition to fw mode switch is on
-		if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+		if (_flight_mode == MC_MODE || _flight_mode == TRANSITION_TO_MC) {
 			// start transition to fw mode
 			/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
 			 * unsafe flying state. */
-			_vtol_schedule.flight_mode = TRANSITION_TO_FW;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_flight_mode = TRANSITION_TO_FW;
+			_transition_start_time = hrt_absolute_time();
 
-		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+		} else if (_flight_mode == FW_MODE) {
 			// in fw mode
-			_vtol_schedule.flight_mode = FW_MODE;
+			_flight_mode = FW_MODE;
 			mc_weight = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+		} else if (_flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 			if (((_params->airspeed_disabled ||
 			      _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) &&
-			     time_since_trans_start > _params->front_trans_time_min) ||
+			     get_time_since_trans_start() > _params->front_trans_time_min) ||
 			    can_transition_on_ground()) {
 
-				_vtol_schedule.flight_mode = FW_MODE;
+				_flight_mode = FW_MODE;
 
 				// don't set pusher throttle here as it's being ramped up elsewhere
 				_trans_finished_ts = hrt_absolute_time();
 			}
-
 		}
 	}
 
@@ -206,7 +196,7 @@ void Standard::update_vtol_state()
 	_mc_throttle_weight = mc_weight;
 
 	// map specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_flight_mode) {
 	case MC_MODE:
 		_vtol_mode = mode::ROTARY_WING;
 		break;
@@ -228,33 +218,32 @@ void Standard::update_vtol_state()
 void Standard::update_transition_state()
 {
 	float mc_weight = 1.0f;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	VtolType::update_transition_state();
 
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 
-	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+	if (_flight_mode == TRANSITION_TO_FW) {
 		if (_params_standard.pusher_ramp_dt <= 0.0f) {
 			// just set the final target throttle value
 			_pusher_throttle = _params->front_trans_throttle;
 
 		} else if (_pusher_throttle <= _params->front_trans_throttle) {
 			// ramp up throttle to the target throttle value
-			_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
+			_pusher_throttle = _params->front_trans_throttle * get_time_since_trans_start() / _params_standard.pusher_ramp_dt;
 		}
 
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
 		    _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
-		    time_since_trans_start > _params->front_trans_time_min) {
+		    get_time_since_trans_start() > _params->front_trans_time_min) {
 			mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
 		} else if (_params->airspeed_disabled) {
-			mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
+			mc_weight = 1.0f - get_time_since_trans_start() / _params->front_trans_time_min;
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 
 		}
@@ -265,15 +254,9 @@ void Standard::update_transition_state()
 		q_sp.copyTo(_v_att_sp->q_d);
 		_v_att_sp->q_d_valid = true;
 
-		// check front transition timeout
-		if (_params->front_trans_timeout > FLT_EPSILON) {
-			if (time_since_trans_start > _params->front_trans_timeout) {
-				// transition timeout occured, abort transition
-				_attc->abort_front_transition("Transition timeout");
-			}
-		}
 
-	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+
+	} else if (_flight_mode == TRANSITION_TO_MC) {
 
 		// maintain FW_PSP_OFF
 		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
@@ -283,16 +266,16 @@ void Standard::update_transition_state()
 
 		_pusher_throttle = 0.0f;
 
-		if (time_since_trans_start >= _params_standard.reverse_delay) {
+		if (get_time_since_trans_start() >= _params_standard.reverse_delay) {
 			// Handle throttle reversal for active breaking
-			float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
+			float thrscale = (get_time_since_trans_start() - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
 			thrscale = math::constrain(thrscale, 0.0f, 1.0f);
 			_pusher_throttle = thrscale * _params->back_trans_throttle;
 		}
 
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_ramp > FLT_EPSILON) {
-			mc_weight = time_since_trans_start / _params_standard.back_trans_ramp;
+			mc_weight = get_time_since_trans_start() / _params_standard.back_trans_ramp;
 
 		}
 
@@ -417,7 +400,7 @@ void Standard::fill_actuator_outputs()
 	// fixed wing controls
 	_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
 
-	if (_vtol_schedule.flight_mode != MC_MODE) {
+	if (_flight_mode != MC_MODE) {
 		// roll
 		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
 			-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
@@ -455,7 +438,7 @@ void Standard::fill_actuator_outputs()
 	}
 
 	// set the fixed wing throttle control
-	if (_vtol_schedule.flight_mode == FW_MODE) {
+	if (_flight_mode == FW_MODE) {
 
 		// take the throttle value commanded by the fw controller
 		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =

diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
@@ -93,14 +93,13 @@ class Standard : public VtolType
 		FW_MODE
 	};
 
-	struct {
-		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
-		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
-	} _vtol_schedule;
 
-	float _pusher_throttle;
-	float _reverse_output;
-	float _airspeed_trans_blend_margin;
+	vtol_mode _flight_mode;			// indicates in which mode the vehicle is in
+
+
+	float _pusher_throttle = 0.0f;
+	float _reverse_output = 0.0f;
+	float _airspeed_trans_blend_margin = 0.0f;
 
 	virtual void parameters_update();
 };