diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 4079b517f3..6dc8d13003 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/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) { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 56255a89e6..c9c5eb5931 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/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 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 6aee2b721e..e5e975a3e8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index efc7fd1d17..f9810d3a2b 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/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; + } } +} - switch (next_state) { +void VtolType::set_alternate_motor_state(const motor_state target_state, const int value) +{ + if (_alternate_motor_state != target_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); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index dac4ff9914..902611a79b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/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); };