diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index c41bc69642..a9d81b0b58 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -291,6 +291,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_T_EXPO"), ¶ms->turtle_motor_expo); param_get(param_find("VOXL_ESC_T_MINF"), ¶ms->turtle_stick_minf); param_get(param_find("VOXL_ESC_T_COSP"), ¶ms->turtle_cosphi); + param_get(param_find("VOXL_ESC_T_ON"), ¶ms->turtle_button_on); param_get(param_find("VOXL_ESC_FUNC1"), ¶ms->function_map[0]); param_get(param_find("VOXL_ESC_FUNC2"), ¶ms->function_map[1]); @@ -363,6 +364,12 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) ret = PX4_ERROR; } + if (params->turtle_button_on < -1 || params->turtle_button_on > 15) { + PX4_ERR("Invalid parameter VOXL_ESC_T_ON. Please verify parameters."); + params->turtle_button_on = -1; + ret = PX4_ERROR; + } + if (params->gpio_ctl_channel < 0 || params->gpio_ctl_channel > 6) { PX4_ERR("Invalid parameter VOXL_ESC_GPIO_CH. Please verify parameters."); params->gpio_ctl_channel = 0; @@ -1495,21 +1502,28 @@ void VoxlEsc::Run() if (!_outputs_on) { - float setpoint = VOXL_ESC_MODE_DISABLED_SETPOINT; + bool activate = false; - if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX1) { - setpoint = _manual_control_setpoint.aux1; + if (_manual_control_setpoint.data_source >= manual_control_setpoint_s::SOURCE_MAVLINK_0 + && _parameters.turtle_button_on >= 0) { + // MAVLink source: use buttons field directly + activate = (_manual_control_setpoint.buttons & (1 << _parameters.turtle_button_on)) != 0; - } else if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX2) { - setpoint = _manual_control_setpoint.aux2; + } else if (_manual_control_setpoint.data_source == manual_control_setpoint_s::SOURCE_RC) { + // RC source: use aux channel as before + float setpoint = VOXL_ESC_MODE_DISABLED_SETPOINT; + + if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX1) { + setpoint = _manual_control_setpoint.aux1; + + } else if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX2) { + setpoint = _manual_control_setpoint.aux2; + } + + activate = (setpoint > VOXL_ESC_MODE_THRESHOLD); } - if (setpoint > VOXL_ESC_MODE_THRESHOLD) { - _turtle_mode_en = true; - - } else { - _turtle_mode_en = false; - } + _turtle_mode_en = activate; } } @@ -1698,6 +1712,7 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_T_EXPO: %" PRId32, _parameters.turtle_motor_expo); PX4_INFO("Params: VOXL_ESC_T_MINF: %f", (double)_parameters.turtle_stick_minf); PX4_INFO("Params: VOXL_ESC_T_COSP: %f", (double)_parameters.turtle_cosphi); + PX4_INFO("Params: VOXL_ESC_T_ON: %" PRId32, _parameters.turtle_button_on); PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging); PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status); diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index a73f26ae10..1f217fcc6d 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -176,6 +176,7 @@ private: int32_t esc_over_temp_threshold{0}; int32_t gpio_ctl_channel{0}; int32_t cmd_type{0}; + int32_t turtle_button_on{-1}; } voxl_esc_params_t; struct EscChan { diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index 6ccfeb60ba..2047c97eaa 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -204,6 +204,24 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_EXPO, 35); */ PARAM_DEFINE_FLOAT(VOXL_ESC_T_COSP, 0.990); +/** + * UART ESC Turtle Mode button index for MAVLink manual control + * + * Specifies which button in the MAVLink MANUAL_CONTROL buttons field + * activates turtle mode. Only used when data source is a MAVLink instance. + * When data source is RC, turtle mode activation uses the AUX channel + * selected by VOXL_ESC_MODE instead. + * Set to -1 to disable turtle mode activation via MAVLink buttons. + * + * @reboot_required true + * + * @group VOXL ESC + * @value -1 Disabled + * @min -1 + * @max 15 + */ +PARAM_DEFINE_INT32(VOXL_ESC_T_ON, -1); + /** * UART ESC verbose logging *