diff --git a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard index 52e1f5d7400b..e249af8d0c2f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol index ec63ac215119..f6d8ff30f2ff 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 index 7f2acf3156cf..4e31e097dbd6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter index dd91d353b53e..47268f3104e2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter index 3f3bab06b774..7c52dc0b91cd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad index ca4288b76d04..8dfa6c85c36b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta b/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta index 209290557d8e..55adc232cbfb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad b/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad index 647d6aeb5708..f527bc3bc927 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger index 4a8b0c93b9cc..cc9ee8537a6e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger index 5d3ebcf5bfcf..3a0f11fc2456 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13010_claire b/ROMFS/px4fmu_common/init.d/airframes/13010_claire index 022bf5885334..1b6c167719d3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13010_claire +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence index d7410eda0da7..0976f0b03d68 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 3671d55eaa8d..75a348e72e56 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 5cb1b4abb277..9159e857bcd6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo b/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo index 7da4a205c84f..8cc0fb8d887b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo +++ b/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 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 80810b2e0cfb..e535e78f507b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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(_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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 9fa425e3eadd..4a381b3ab65c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/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; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index c6684f5977da..4e94b7632ff3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -39,17 +39,6 @@ * @author Sander Smeets */ -/** - * 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. * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 475dddfc4a0e..0765f50d4e99 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -46,6 +46,7 @@ #include #include + 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,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++) { - 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; } } @@ -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++) { - 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; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index ee4a62dcc42b..5b743c0c59b4 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/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); };