Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

vtol_att_control: do not manipulate PWM outputs if SYS_CTRL_ALLOC == 1

Browse Source 
Not required anymore
This commit is contained in:
Beat Küng
2021-11-29 15:08:51 +01:00
committed by Daniel Agar
parent 43e15148f6
commit b5c2cdf6c4
4 changed files with 82 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+22 -12
View File
@@ -63,8 +63,18 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params.idle_pwm_mc = PWM_DEFAULT_MIN;
_params.vtol_motor_id = 0;
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
_params_handles.sys_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
_params.ctrl_alloc = 0;
param_get(_params_handles.sys_ctrl_alloc, &_params.ctrl_alloc);
if (_params.ctrl_alloc != 1) {
// these are not used with dynamic control allocation
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
}
_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");
@@ -84,7 +94,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
@@ -93,7 +102,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.pitch_min_rad = param_find("VT_PTCH_MIN");
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
@@ -265,12 +273,17 @@ VtolAttitudeControl::parameters_update()
{
float v;
int32_t l;
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
/* 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);
if (_params.ctrl_alloc != 1) {
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
param_get(_params_handles.vt_mc_on_fmu, &l);
_params.vt_mc_on_fmu = l;
/* vtol motor count */
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
}
/* vtol fw permanent stabilization */
param_get(_params_handles.vtol_fw_permanent_stab, &l);
@@ -355,9 +368,6 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
param_get(_params_handles.vt_mc_on_fmu, &l);
_params.vt_mc_on_fmu = l;
param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
src/modules/vtol_att_control/vtol_att_control_main.h
+1
View File
@@ -235,6 +235,7 @@ private:
param_t vt_forward_thrust_enable_mode;
param_t mpc_land_alt1;
param_t mpc_land_alt2;
param_t sys_ctrl_alloc;
} _params_handles{};
hrt_abstime _last_run_timestamp{0};
src/modules/vtol_att_control/vtol_type.cpp
+58 -36
View File
@@ -83,50 +83,52 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
bool VtolType::init()
{
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
if (_params->ctrl_alloc != 1) {
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_ERR("can't open %s", dev);
return false;
}
if (fd < 0) {
PX4_ERR("can't open %s", dev);
return false;
}
int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);
_current_max_pwm_values = _max_mc_pwm_values;
int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);
_current_max_pwm_values = _max_mc_pwm_values;
if (ret != PX4_OK) {
PX4_ERR("failed getting max values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting min values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting disarmed values");
px4_close(fd);
return false;
}
if (ret != PX4_OK) {
PX4_ERR("failed getting max values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting min values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting disarmed values");
px4_close(fd);
return false;
}
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);
_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);
// 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);
}
}
}
@@ -322,6 +324,10 @@ void VtolType::check_quadchute_condition()
bool VtolType::set_idle_mc()
{
if (_params->ctrl_alloc == 1) {
return true;
}
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values {};
@@ -341,6 +347,10 @@ bool VtolType::set_idle_mc()
bool VtolType::set_idle_fw()
{
if (_params->ctrl_alloc == 1) {
return true;
}
struct pwm_output_values pwm_values {};
for (int i = 0; i < num_outputs_max; i++) {
@@ -390,12 +400,20 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
void VtolType::set_all_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
return;
}
set_main_motor_state(target_state, value);
set_alternate_motor_state(target_state, value);
}
void VtolType::set_main_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
return;
}
if (_main_motor_state != target_state || target_state == motor_state::VALUE) {
if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) {
@@ -406,6 +424,10 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
return;
}
if (_alternate_motor_state != target_state || target_state == motor_state::VALUE) {
if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) {
src/modules/vtol_att_control/vtol_type.h
+1
View File
@@ -48,6 +48,7 @@
#include <drivers/drv_pwm_output.h>
struct Params {
int32_t ctrl_alloc;
int32_t idle_pwm_mc; // pwm value for idle in mc mode
int32_t vtol_motor_id;
int32_t vtol_type;