vtol_att_control: use VT_MOT_ID instead of VT_MOT_COUNT

- VT_MOT_COUNT assumed that motors were always the first outputs. This
does not have to be true always. VT_MOT_ID allows to specify precisely how
many motors we have for hovering and to which channel they are connected.

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

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -65,10 +65,10 @@ VtolAttitudeControl::VtolAttitudeControl()
 	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
 
 	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
-	_params.vtol_motor_count = 0;
+	_params.vtol_motor_id = 0;
 
 	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
-	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
 	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
 	_params_handles.vtol_type = param_find("VT_TYPE");
 	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
@@ -269,7 +269,7 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
 
 	/* vtol motor count */
-	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
+	param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
 
 	/* vtol fw permanent stabilization */
 	param_get(_params_handles.vtol_fw_permanent_stab, &l);
@@ -326,14 +326,6 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.diff_thrust_scale, &v);
 	_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 
-	// standard vtol always needs to turn all mc motors off when going into fixed wing mode
-	// normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code
-	// did not use the interface of the VtolType class to disable motors we will have users flying  around with a wrong
-	// parameter value. Therefore, explicitly set it here such that all motors will be disabled as expected.
-	if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
-		_params.fw_motors_off = 12345678;
-	}
-
 	// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
 	_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
 

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -175,7 +175,7 @@ private:
 
 	struct {
 		param_t idle_pwm_mc;
-		param_t vtol_motor_count;
+		param_t vtol_motor_id;
 		param_t vtol_fw_permanent_stab;
 		param_t vtol_type;
 		param_t elevons_mc_lock;

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -39,17 +39,6 @@
  * @author Sander Smeets <sander@droneslab.com>
  */
 
-/**
- * VTOL number of engines
- *
- * @min 0
- * @max 8
- * @increment 1
- * @decimal 0
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_MOT_COUNT, 0);
-
 /**
  * Idle speed of VTOL when in multicopter mode
  *
@@ -291,6 +280,17 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
  */
 PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
 
+/**
+ * The channel number of motors which provide lift during hover.
+ *
+ * @min 0
+ * @max 12345678
+ * @increment 1
+ * @decimal 0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_MOT_ID, 0);
+
 /**
  * Differential thrust in forwards flight.
  *

src/modules/vtol_att_control/vtol_type.cpp

@@ -46,6 +46,7 @@
 #include <px4_defines.h>
 #include <matrix/math.hpp>
 
+
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_attc(att_controller),
 	_vtol_mode(mode::ROTARY_WING)
@@ -248,8 +249,8 @@ bool VtolType::set_idle_mc()
 	struct pwm_output_values pwm_values;
 	memset(&pwm_values, 0, sizeof(pwm_values));
 
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
+	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_motor_off_channel(i)) {
+		if (is_channel_set(i, _params->vtol_motor_id)) {
 			pwm_values.values[i] = pwm_value;
 
 		} else {
@@ -268,8 +269,8 @@ bool VtolType::set_idle_fw()
 
 	memset(&pwm_values, 0, sizeof(pwm_values));
 
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
+	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_motor_off_channel(i)) {
+		if (is_channel_set(i, _params->vtol_motor_id)) {
 			pwm_values.values[i] = PWM_MOTOR_OFF;
 
 		} else {
@@ -315,10 +316,10 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
 motor_state VtolType::set_motor_state(const motor_state current_state, const motor_state next_state, const int value)
 {
 	struct pwm_output_values pwm_values = {};
-	pwm_values.channel_count = _params->vtol_motor_count;
+	pwm_values.channel_count = num_outputs_max;
 
 	// per default all motors are running
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
+	for (int i = 0; i < num_outputs_max; i++) {
 		pwm_values.values[i] = _max_mc_pwm_values.values[i];
 	}
 
@@ -327,8 +328,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 		break;
 
 	case motor_state::DISABLED:
-		for (int i = 0; i < _params->vtol_motor_count; i++) {
+		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_motor_off_channel(i)) {
+			if (is_channel_set(i, _params->fw_motors_off)) {
 				pwm_values.values[i] = _disarmed_pwm_values.values[i];
 			}
 		}
@@ -337,8 +338,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 
 	case motor_state::IDLE:
 
-		for (int i = 0; i < _params->vtol_motor_count; i++) {
+		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_motor_off_channel(i)) {
+			if (is_channel_set(i, _params->vtol_motor_id)) {
 				pwm_values.values[i] = _params->idle_pwm_mc;
 			}
 		}
@@ -346,8 +347,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 		break;
 
 	case motor_state::VALUE:
-		for (int i = 0; i < _params->vtol_motor_count; i++) {
+		for (int i = 0; i < num_outputs_max; i++) {
-			if (is_motor_off_channel(i)) {
+			if (is_channel_set(i, _params->fw_motors_off)) {
 				pwm_values.values[i] = value;
 			}
 		}
@@ -363,14 +364,13 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
 	}
 }
 
-bool VtolType::is_motor_off_channel(const int channel)
+bool VtolType::is_channel_set(const int channel, const int target)
 {
 	int channel_bitmap = 0;
 
 	int tmp;
-	int channels = _params->fw_motors_off;
+	int channels = target;
 
-	static constexpr int num_outputs_max = 8;
 
 	for (int i = 0; i < num_outputs_max; ++i) {
 		tmp = channels % 10;

src/modules/vtol_att_control/vtol_type.h

@@ -49,7 +49,7 @@
 
 struct Params {
 	int32_t idle_pwm_mc;			// pwm value for idle in mc mode
-	int32_t vtol_motor_count;		// number of motors
+	int32_t vtol_motor_id;
 	int32_t vtol_type;
 	bool elevons_mc_lock;		// lock elevons in multicopter mode
 	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
@@ -173,6 +173,8 @@ protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
 
+	static constexpr const int num_outputs_max = 8;
+
 	struct vehicle_attitude_s		*_v_att;				//vehicle attitude
 	struct vehicle_attitude_setpoint_s	*_v_att_sp;			//vehicle attitude setpoint
 	struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp;	// virtual mc attitude setpoint
@@ -266,13 +268,14 @@ private:
 	bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type);
 
 	/**
-	 * @brief      Determines if this channel is one selected by VT_FW_MOT_OFFID
+	 * @brief      Determines if channel is set in target.
 	 *
 	 * @param[in]  channel  The channel
+	 * @param[in]  target  	The target to check on.
 	 *
 	 * @return     True if motor off channel, False otherwise.
 	 */
-	bool is_motor_off_channel(const int channel);
+	bool is_channel_set(const int channel, const int target);
 
 };