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 80810b2e0cf..e535e78f507 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 9fa425e3ead..4a381b3ab65 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 @@ 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; 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 c6684f5977d..4e94b7632ff 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 475dddfc4a0..0765f50d4e9 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 ee4a62dcc42..5b743c0c59b 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 @@ 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); };