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 bc50a6703a..11f96514b9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); 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 8929cb3bb0..a83dff2cc8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0b32d0d828..3cb501313e 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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)) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5123e64dcc..fb34aac380 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -48,6 +48,7 @@ #include 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;