vtol: refactor of maximum pwm value settings for main and alternate motors

- main motors are the ones which are meant to be used as propulsion in fw mode
- alternate motors are the ones usually meant to be switched off in fw mode

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

src/modules/vtol_att_control/standard.cpp

@@ -305,10 +305,7 @@ void Standard::update_transition_state()
 
 		}
 
-		// in back transition we need to start the MC motors again
-		if (_motor_state != motor_state::ENABLED) {
-			_motor_state = set_motor_state(_motor_state, motor_state::ENABLED);
-		}
+		set_all_motor_state(motor_state::ENABLED);
 
 		// set idle speed for MC actuators
 		if (!_flag_idle_mc) {

src/modules/vtol_att_control/tiltrotor.cpp

@@ -286,9 +286,7 @@ void Tiltrotor::update_transition_state()
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
 		// for the first part of the transition all rotors are enabled
-		if (_motor_state != motor_state::ENABLED) {
-			_motor_state = set_motor_state(_motor_state, motor_state::ENABLED);
-		}
+		set_all_motor_state(motor_state::ENABLED);
 
 		// tilt rotors forward up to certain angle
 		if (_tilt_control <= _params_tiltrotor.tilt_transition) {
@@ -338,16 +336,14 @@ void Tiltrotor::update_transition_state()
 				      (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
 
 
-		_motor_state = set_motor_state(_motor_state, motor_state::VALUE, ramp_down_value);
+		set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
 
 
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 		// turn on all MC motors
-		if (_motor_state != motor_state::ENABLED) {
-			_motor_state = set_motor_state(_motor_state, motor_state::ENABLED);
-		}
+		set_all_motor_state(motor_state::ENABLED);
 
 
 		// set idle speed for rotary wing mode

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -242,6 +242,7 @@ VtolAttitudeControl::parameters_update()
 
 	/* vtol motor count */
 	param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
+	param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
 
 	/* vtol fw permanent stabilization */
 	param_get(_params_handles.vtol_fw_permanent_stab, &l);
@@ -294,7 +295,6 @@ VtolAttitudeControl::parameters_update()
 	_params.airspeed_disabled = l != 0;
 	param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
 	param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
-	param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
 	param_get(_params_handles.diff_thrust, &_params.diff_thrust);
 
 	param_get(_params_handles.diff_thrust_scale, &v);

src/modules/vtol_att_control/vtol_type.cpp

@@ -77,6 +77,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	for (auto &pwm_disarmed : _disarmed_pwm_values.values) {
 		pwm_disarmed = PWM_MOTOR_OFF;
 	}
+
+	_current_max_pwm_values = _max_mc_pwm_values;
 }
 
 bool VtolType::init()
@@ -117,6 +119,17 @@ bool VtolType::init()
 
 	px4_close(fd);
 
+	_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id);
+	_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off);
+
+
+	// in order to get the main motors we take all motors and clear the alternate motor bits
+	for (int i = 0; i < 8; i++) {
+		if (_alternate_motor_channel_bitmap & (1 << i)) {
+			_main_motor_channel_bitmap &= ~(1 << i);
+		}
+	}
+
 	return true;
 
 }
@@ -127,9 +140,7 @@ void VtolType::update_mc_state()
 		_flag_idle_mc = set_idle_mc();
 	}
 
-	if (_motor_state != motor_state::ENABLED) {
-		_motor_state = VtolType::set_motor_state(_motor_state, motor_state::ENABLED);
-	}
+	VtolType::set_all_motor_state(motor_state::ENABLED);
 
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
@@ -146,9 +157,7 @@ void VtolType::update_fw_state()
 		_flag_idle_mc = !set_idle_fw();
 	}
 
-	if (_motor_state != motor_state::DISABLED) {
-		_motor_state = VtolType::set_motor_state(_motor_state, motor_state::DISABLED);
-	}
+	VtolType::set_alternate_motor_state(motor_state::DISABLED);
 
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
@@ -291,7 +300,7 @@ bool VtolType::set_idle_mc()
 	struct pwm_output_values pwm_values {};
 
 	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_channel_set(i, _params->vtol_motor_id)) {
+		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
 			pwm_values.values[i] = pwm_value;
 
 		} else {
@@ -309,7 +318,7 @@ bool VtolType::set_idle_fw()
 	struct pwm_output_values pwm_values {};
 
 	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_channel_set(i, _params->vtol_motor_id)) {
+		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
 			pwm_values.values[i] = PWM_MOTOR_OFF;
 
 		} else {
@@ -353,24 +362,48 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
 	return true;
 }
 
-motor_state VtolType::set_motor_state(const motor_state current_state, const motor_state next_state, const int value)
+void VtolType::set_all_motor_state(const motor_state target_state, const int value)
 {
-	struct pwm_output_values pwm_values = {};
-	pwm_values.channel_count = num_outputs_max;
+	set_main_motor_state(target_state, value);
+	set_alternate_motor_state(target_state, value);
+}
 
-	// per default all motors are running
-	for (int i = 0; i < num_outputs_max; i++) {
-		pwm_values.values[i] = _max_mc_pwm_values.values[i];
+void VtolType::set_main_motor_state(const motor_state target_state, const int value)
+{
+	if (_main_motor_state != target_state) {
+
+		if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) {
+			_main_motor_state = target_state;
+		}
 	}
+}
+
+void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
+{
+	if (_alternate_motor_state != target_state) {
 
-	switch (next_state) {
+		if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) {
+			_alternate_motor_state = target_state;
+		}
+	}
+}
+
+bool VtolType::set_motor_state(const motor_state target_state, const int32_t channel_bitmap,  const int value)
+{
+	switch (target_state) {
 	case motor_state::ENABLED:
+		for (int i = 0; i < num_outputs_max; i++) {
+			if (is_channel_set(i, channel_bitmap)) {
+				_current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i];
+			}
+		}
+
 		break;
 
 	case motor_state::DISABLED:
 		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_channel_set(i, _params->fw_motors_off)) {
-				pwm_values.values[i] = _disarmed_pwm_values.values[i];
+			if (is_channel_set(i, channel_bitmap)) {
+				_current_max_pwm_values.values[i] = _disarmed_pwm_values.values[i];
 			}
 		}
 
@@ -379,8 +412,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 	case motor_state::IDLE:
 
 		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_channel_set(i, _params->vtol_motor_id)) {
-				pwm_values.values[i] = _params->idle_pwm_mc;
+			if (is_channel_set(i, channel_bitmap)) {
+				_current_max_pwm_values.values[i] = _params->idle_pwm_mc;
 			}
 		}
 
@@ -388,42 +421,43 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 
 	case motor_state::VALUE:
 		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_channel_set(i, _params->fw_motors_off)) {
-				pwm_values.values[i] = value;
+			if (is_channel_set(i, channel_bitmap)) {
+				_current_max_pwm_values.values[i] = value;
 			}
 		}
 
 		break;
 	}
 
-	if (apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MAXIMUM)) {
-		return next_state;
+	_current_max_pwm_values.channel_count = num_outputs_max;
 
-	} else {
-		return current_state;
-	}
+	return apply_pwm_limits(_current_max_pwm_values, pwm_limit_type::TYPE_MAXIMUM);
 }
 
-bool VtolType::is_channel_set(const int channel, const int target)
+int VtolType::generate_bitmap_from_channel_numbers(const int channels)
 {
 	int channel_bitmap = 0;
+	int channel_numbers = channels;
 
 	int tmp;
-	int channels = target;
-
 
 	for (int i = 0; i < num_outputs_max; ++i) {
-		tmp = channels % 10;
+		tmp = channel_numbers % 10;
 
 		if (tmp == 0) {
 			break;
 		}
 
 		channel_bitmap |= 1 << (tmp - 1);
-		channels = channels / 10;
+		channel_numbers = channel_numbers / 10;
 	}
 
-	return (channel_bitmap >> channel) & 1;
+	return channel_bitmap;
+}
+
+bool VtolType::is_channel_set(const int channel, const int bitmap)
+{
+	return bitmap & (1 << channel);
 }
 
 

src/modules/vtol_att_control/vtol_type.h

@@ -235,7 +235,8 @@ protected:
 	bool _tecs_running = false;
 	hrt_abstime _tecs_running_ts = 0;
 
-	motor_state _motor_state = motor_state::DISABLED;
+	motor_state _main_motor_state = motor_state::DISABLED;
+	motor_state _alternate_motor_state = motor_state::DISABLED;
 
 	hrt_abstime _last_loop_ts = 0;
 	float _transition_dt = 0;
@@ -260,18 +261,11 @@ protected:
 	 */
 	bool set_idle_fw();
 
+	void set_all_motor_state(const motor_state target_state, const int value = 0);
 
-	/**
-	 * @brief      Sets state of a selection of motors, see struct motor_state
-	 *
-	 * @param[in]  current_state  The current motor state
-	 * @param[in]  next_state     The next state
-	 * @param[in]  value          Desired pwm value if next_state =
-	 *                            motor_state::VALUE
-	 *
-	 * @return     next_state if succesfull, otherwise current_state
-	 */
-	motor_state set_motor_state(const motor_state current_state, const motor_state next_state, const int value = 0);
+	void set_main_motor_state(const motor_state target_state, const int value = 0);
+
+	void set_alternate_motor_state(const motor_state target_state, const int value = 0);
 
 	float update_and_get_backtransition_pitch_sp();
 
@@ -285,6 +279,11 @@ private:
 	struct pwm_output_values _max_mc_pwm_values {};
 	struct pwm_output_values _disarmed_pwm_values {};
 
+	struct pwm_output_values _current_max_pwm_values {};
+
+	int32_t _main_motor_channel_bitmap = 0;
+	int32_t _alternate_motor_channel_bitmap = 0;
+
 	/**
 	 * @brief      Adjust minimum/maximum pwm values for the output channels.
 	 *
@@ -296,14 +295,19 @@ private:
 	bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type);
 
 	/**
-	 * @brief      Determines if channel is set in target.
+	 * @brief      Determines if channel is set in a bitmap.
 	 *
 	 * @param[in]  channel  The channel
-	 * @param[in]  target  	The target to check on.
+	 * @param[in]  bitmap  	The bitmap to check on.
 	 *
-	 * @return     True if motor off channel, False otherwise.
+	 * @return     True if set, false otherwise.
 	 */
-	bool is_channel_set(const int channel, const int target);
+	bool is_channel_set(const int channel, const int bitmap);
+
+	// generates a bitmap from a number format, e.g. 1235 -> first, second, third and fifth bits should be set.
+	int generate_bitmap_from_channel_numbers(const int channels);
+
+	bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap,  const int value);
 
 };