VTOL motor output handling

ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard

@@ -26,7 +26,8 @@ then
 	param set PWM_RATE 400
 
 	param set VT_TYPE 2
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 fi
 
 set MAV_TYPE 22

ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol

@@ -35,7 +35,7 @@ then
 
 	param set VT_IDLE_PWM_MC  1080
 	param set VT_ELEV_MC_LOCK 0
-	param set VT_MOT_COUNT 2
+	param set VT_MOT_ID 12
 	param set VT_TYPE 0
 fi
 

ROMFS/px4fmu_common/init.d/airframes/13002_firefly6

@@ -42,7 +42,7 @@ then
 
 	param set VT_FW_MOT_OFFID 34
 	param set VT_IDLE_PWM_MC 1080
-	param set VT_MOT_COUNT 6
+	param set VT_MOT_COUNT 123456
 	param set VT_TILT_MC 0.08
 	param set VT_TILT_TRANS 0.5
 	param set VT_TILT_FW 0.9

ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter

@@ -15,7 +15,7 @@ then
 	param set PWM_MAX 2000
 	param set PWM_RATE 400
 
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
 	param set VT_IDLE_PWM_MC  1080
 	param set VT_TYPE 0
 	param set VT_ELEV_MC_LOCK 1

ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter

@@ -26,7 +26,7 @@ then
 	param set PWM_MAX 2000
 	param set PWM_RATE 400
 
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_COUNT 1234
 	param set VT_IDLE_PWM_MC  1080
 	param set VT_TYPE 0
 	param set VT_ELEV_MC_LOCK 1

ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad

@@ -58,7 +58,8 @@ then
 	param set VT_ARSP_BLEND 6
 	param set VT_ARSP_TRANS 12
 	param set VT_F_TRANS_THR 0.75
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_COUNT 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_IDLE_PWM_MC 1080
 	param set VT_TYPE 2
 fi

ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta

@@ -45,7 +45,8 @@ then
 	param set PWM_AUX_DIS3 950
 	param set PWM_RATE 400
 
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_F_TRANS_THR 0.75
 	param set VT_IDLE_PWM_MC 1080
 	param set VT_TYPE 2

ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad

@@ -35,7 +35,8 @@ then
 	param set PWM_RATE 400
 
 	param set VT_F_TRANS_THR 0.75
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_IDLE_PWM_MC 1080
 	param set VT_TYPE 2
 fi

ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger

@@ -53,7 +53,8 @@ then
 	param set VT_B_TRANS_DUR 4
 	param set VT_F_TRANS_THR 0.75
 	param set VT_IDLE_PWM_MC 1080
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_TYPE 2
 fi
 

ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger

@@ -72,7 +72,8 @@ then
 	param set VT_B_TRANS_DUR  4
 	param set VT_F_TRANS_THR    0.6
 	param set VT_IDLE_PWM_MC 1180
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_TRANS_MIN_TM 5
 	param set VT_TRANS_TIMEOUT    30
 	param set VT_TYPE 2

ROMFS/px4fmu_common/init.d/airframes/13010_claire

@@ -22,7 +22,7 @@ then
 	param set PWM_MAX 2000
 	param set PWM_RATE 400
 
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
 	param set VT_FW_MOT_OFFID 13
 	param set VT_IDLE_PWM_MC 1080
 	param set VT_TILT_FW 0.9

ROMFS/px4fmu_common/init.d/airframes/13012_convergence

@@ -69,7 +69,7 @@ then
 	param set VT_FW_MOT_OFFID 3
 	param set VT_FW_PERM_STAB 0
 	param set VT_IDLE_PWM_MC 1200
-	param set VT_MOT_COUNT 3
+	param set VT_MOT_ID 123
 	param set VT_TILT_FW  1
 	param set VT_TILT_MC  0
 	param set VT_TILT_TRANS   0.45

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -133,7 +133,8 @@ then
 	param set SER_TEL2_BAUD 57600
 
 	param set VT_TYPE 2
-	param set VT_MOT_COUNT 4
+	param set VT_MOT_ID 1234
+	param set VT_FW_MOT_OFFID 1234
 	param set VT_F_TRANS_THR 1
 	param set VT_DWN_PITCH_MAX 8
 	param set VT_FW_QC_P 55

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -67,7 +67,7 @@ then
 	param set VT_F_TRANS_THR 0.85
 	param set VT_F_TR_OL_TM 7
 	param set VT_IDLE_PWM_MC 1000
-	param set VT_MOT_COUNT 8
+	param set VT_MOT_ID 5678
 	param set VT_PSHER_RMP_DT 2
 	param set VT_TRANS_MIN_TM 6
 	param set VT_TYPE 2

ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo

@@ -30,7 +30,8 @@ then
 	param set PWM_RATE 400
 
 	param set VT_TYPE 2
-	param set VT_MOT_COUNT 8
+	param set VT_MOT_ID 12345678
+	param set VT_FW_MOT_OFFID 12345678
 fi
 
 set MAV_TYPE 22

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 @@ class VtolAttitudeControl
 
 	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 <[email protected]>
  */
 
-/**
- * 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++) {
-		if (is_motor_off_channel(i)) {
+	for (int i = 0; i < num_outputs_max; 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++) {
-		if (is_motor_off_channel(i)) {
+	for (int i = 0; i < num_outputs_max; 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++) {
-			if (is_motor_off_channel(i)) {
+		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];
 			}
 		}
@@ -337,17 +338,17 @@ 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++) {
-			if (is_motor_off_channel(i)) {
+		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;
 			}
 		}
 
 		break;
 
 	case motor_state::VALUE:
-		for (int i = 0; i < _params->vtol_motor_count; i++) {
-			if (is_motor_off_channel(i)) {
+		for (int i = 0; i < num_outputs_max; 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 @@ class VtolType
 	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 @@ class VtolType
 	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);
 
 };