dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vtol: refactor of maximum pwm value settings for main and alternate motors 

- main motors are the ones which are meant to be used as propulsion in fw mode
- alternate motors are the ones usually meant to be switched off in fw mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-08-04 18:21:34 +03:00 committed by Silvan Fuhrer
parent bc1e9f72f7
commit 0871f0f52d
5 changed files with 91 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/vtol_att_control/standard.cpp
View File

@ -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) {

10
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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

2
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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);

98
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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
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;
}
}
}
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++) {
pwm_values.values[i] = _max_mc_pwm_values.values[i];
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i];
}
}
switch (next_state) {
case motor_state::ENABLED:
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);
}

36
src/modules/vtol_att_control/vtol_type.h
View File

@ -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);
};