From 6f718cd48d9745e4f8347cb918ccc816860d24cb Mon Sep 17 00:00:00 2001 From: modaltb <50114502+modaltb@users.noreply.github.com> Date: Mon, 16 Jan 2023 18:46:37 -0800 Subject: [PATCH] drivers/actuators/modalai_esc: update to use mixer module and control allocator properly - update motor mapping to use new UART_ESC_FUNC* auto generated params - add support for actuator_test msg to support Actuator Testing in QGC - modalai_fc-vX targets start driver if configured - keep track of ESC spin direction in own param - set ramp up param in MixerOutput to false --- boards/modalai/fc-v1/init/rc.board_sensors | 5 ++ boards/modalai/fc-v2/init/rc.board_sensors | 9 +-- .../actuators/modalai_esc/CMakeLists.txt | 2 + .../actuators/modalai_esc/modalai_esc.cpp | 71 +++++++++++++---- .../actuators/modalai_esc/modalai_esc.hpp | 7 +- .../modalai_esc/modalai_esc_params.c | 77 +++++++++++-------- src/drivers/actuators/modalai_esc/module.yaml | 26 +++++++ 7 files changed, 145 insertions(+), 52 deletions(-) create mode 100644 src/drivers/actuators/modalai_esc/module.yaml diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 8261c9b5c2..9a1e729cd0 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -3,6 +3,11 @@ # ModalAI FC-v1 specific board sensors init #------------------------------------------------------------------------------ +if param greater UART_ESC_CONFIG 0 +then + modalai_esc start +fi + board_adc start # Start Digital power monitors diff --git a/boards/modalai/fc-v2/init/rc.board_sensors b/boards/modalai/fc-v2/init/rc.board_sensors index da0a0a8a79..ddc13ad5ef 100644 --- a/boards/modalai/fc-v2/init/rc.board_sensors +++ b/boards/modalai/fc-v2/init/rc.board_sensors @@ -3,11 +3,10 @@ # ModalAI FC-v2 specific board sensors init #------------------------------------------------------------------------------ -# VOXL ESC - TODO -#if param greater UART_ESC_CONFIG 0 -#then -# modalai_esc -d /dev/ttyS4 start -#fi +if param greater UART_ESC_CONFIG 0 +then + modalai_esc start +fi board_adc start diff --git a/src/drivers/actuators/modalai_esc/CMakeLists.txt b/src/drivers/actuators/modalai_esc/CMakeLists.txt index 1190937583..b3dd409b04 100644 --- a/src/drivers/actuators/modalai_esc/CMakeLists.txt +++ b/src/drivers/actuators/modalai_esc/CMakeLists.txt @@ -48,4 +48,6 @@ px4_add_module( DEPENDS px4_work_queue mixer_module + MODULE_CONFIG + module.yaml ) diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.cpp b/src/drivers/actuators/modalai_esc/modalai_esc.cpp index e5c2832392..9c4a587ec5 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc.cpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc.cpp @@ -36,8 +36,6 @@ #include "modalai_esc.hpp" #include "modalai_esc_serial.hpp" -#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc" - // utility for running on VOXL and using driver as a bridge #define MODALAI_ESC_VOXL_BRIDGE_PORT "/dev/ttyS4" @@ -47,7 +45,8 @@ const char *_device; ModalaiEsc::ModalaiEsc() : - OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODALAI_ESC_DEFAULT_PORT)), + _mixing_output{"UART_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { @@ -123,18 +122,33 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map) { int ret = PX4_OK; + // initialize out + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + params->function_map[i] = (int)OutputFunction::Disabled; + params->direction_map[i] = 0; + params->motor_map[i] = 0; + } + param_get(param_find("UART_ESC_CONFIG"), ¶ms->config); param_get(param_find("UART_ESC_MODE"), ¶ms->mode); + param_get(param_find("UART_ESC_BAUD"), ¶ms->baud_rate); + param_get(param_find("UART_ESC_T_PERC"), ¶ms->turtle_motor_percent); param_get(param_find("UART_ESC_T_DEAD"), ¶ms->turtle_motor_deadband); param_get(param_find("UART_ESC_T_EXPO"), ¶ms->turtle_motor_expo); param_get(param_find("UART_ESC_T_MINF"), ¶ms->turtle_stick_minf); param_get(param_find("UART_ESC_T_COSP"), ¶ms->turtle_cosphi); - param_get(param_find("UART_ESC_BAUD"), ¶ms->baud_rate); - param_get(param_find("UART_ESC_MOTOR1"), ¶ms->motor_map[0]); - param_get(param_find("UART_ESC_MOTOR2"), ¶ms->motor_map[1]); - param_get(param_find("UART_ESC_MOTOR3"), ¶ms->motor_map[2]); - param_get(param_find("UART_ESC_MOTOR4"), ¶ms->motor_map[3]); + + param_get(param_find("UART_ESC_FUNC1"), ¶ms->function_map[0]); + param_get(param_find("UART_ESC_FUNC2"), ¶ms->function_map[1]); + param_get(param_find("UART_ESC_FUNC3"), ¶ms->function_map[2]); + param_get(param_find("UART_ESC_FUNC4"), ¶ms->function_map[3]); + + param_get(param_find("UART_ESC_SDIR1"), ¶ms->direction_map[0]); + param_get(param_find("UART_ESC_SDIR2"), ¶ms->direction_map[1]); + param_get(param_find("UART_ESC_SDIR3"), ¶ms->direction_map[2]); + param_get(param_find("UART_ESC_SDIR4"), ¶ms->direction_map[3]); + param_get(param_find("UART_ESC_RPM_MIN"), ¶ms->rpm_min); param_get(param_find("UART_ESC_RPM_MAX"), ¶ms->rpm_max); @@ -174,6 +188,18 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map) ret = PX4_ERROR; } + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { + PX4_ERR("Invalid parameter UART_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); + params->function_map[i] = 0; + ret = PX4_ERROR; + + } else { + // Motor function IDs start at 100, Motor1 = 101, Motor2 = 102... + params->motor_map[i] = params->function_map[i] - 100; + } + } + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { if (params->motor_map[i] == MODALAI_ESC_OUTPUT_DISABLED || params->motor_map[i] < -(MODALAI_ESC_OUTPUT_CHANNELS) || @@ -185,7 +211,7 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map) /* Can map -4 to 4, 0 being disabled. Negative represents reverse direction */ map[i].number = abs(params->motor_map[i]); - map[i].direction = (params->motor_map[i] > 0) ? 1 : -1; + map[i].direction = (params->direction_map[i] > 0) ? -1 : 1; } return ret; @@ -769,8 +795,11 @@ int ModalaiEsc::update_params() ret = load_params(&_parameters, (ch_assign_t *)&_output_map); if (ret == PX4_OK) { + _mixing_output.setAllDisarmedValues(0); + _mixing_output.setAllFailsafeValues(0); _mixing_output.setAllMinValues(_parameters.rpm_min); _mixing_output.setAllMaxValues(_parameters.rpm_max); + _rpm_fullscale = _parameters.rpm_max - _parameters.rpm_min; } @@ -1318,6 +1347,13 @@ void ModalaiEsc::Run() } } + if (!_outputs_on) { + if (_actuator_test_sub.updated()) { + // values are set in ActuatorTest::update, we just need to enable outputs to let them through + _outputs_on = true; + } + } + /* Don't process commands if outputs on */ if (!_outputs_on) { if (_current_cmd.valid()) { @@ -1438,10 +1474,17 @@ int ModalaiEsc::print_status() PX4_INFO("Params: UART_ESC_CONFIG: %li", _parameters.config); PX4_INFO("Params: UART_ESC_BAUD: %li", _parameters.baud_rate); - PX4_INFO("Params: UART_ESC_MOTOR1: %li", _parameters.motor_map[0]); - PX4_INFO("Params: UART_ESC_MOTOR2: %li", _parameters.motor_map[1]); - PX4_INFO("Params: UART_ESC_MOTOR3: %li", _parameters.motor_map[2]); - PX4_INFO("Params: UART_ESC_MOTOR4: %li", _parameters.motor_map[3]); + + PX4_INFO("Params: UART_ESC_FUNC1: %li", _parameters.function_map[0]); + PX4_INFO("Params: UART_ESC_FUNC2: %li", _parameters.function_map[1]); + PX4_INFO("Params: UART_ESC_FUNC3: %li", _parameters.function_map[2]); + PX4_INFO("Params: UART_ESC_FUNC4: %li", _parameters.function_map[3]); + + PX4_INFO("Params: UART_ESC_SDIR1: %li", _parameters.direction_map[0]); + PX4_INFO("Params: UART_ESC_SDIR2: %li", _parameters.direction_map[1]); + PX4_INFO("Params: UART_ESC_SDIR3: %li", _parameters.direction_map[2]); + PX4_INFO("Params: UART_ESC_SDIR4: %li", _parameters.direction_map[3]); + PX4_INFO("Params: UART_ESC_RPM_MIN: %li", _parameters.rpm_min); PX4_INFO("Params: UART_ESC_RPM_MAX: %li", _parameters.rpm_max); @@ -1449,6 +1492,8 @@ int ModalaiEsc::print_status() for( int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++){ PX4_INFO("-- ID: %i", i); + PX4_INFO(" Motor: %i", _output_map[i].number); + PX4_INFO(" Direction: %i", _output_map[i].direction); PX4_INFO(" State: %i", _esc_chans[i].state); PX4_INFO(" Requested: %i RPM", _esc_chans[i].rate_req); PX4_INFO(" Measured: %i RPM", _esc_chans[i].rate_meas); diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.hpp b/src/drivers/actuators/modalai_esc/modalai_esc.hpp index 7dc737b493..dd1c248d18 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc.hpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include "modalai_esc_serial.hpp" @@ -146,7 +147,9 @@ private: int32_t baud_rate{MODALAI_ESC_DEFAULT_BAUD}; int32_t rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN}; int32_t rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX}; + int32_t function_map[MODALAI_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0}; int32_t motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; + int32_t direction_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1}; } uart_esc_params_t; struct EscChan { @@ -177,7 +180,7 @@ private: } led_rsc_t; ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; - MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + MixingOutput _mixing_output; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; @@ -189,6 +192,7 @@ private: uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; //uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; @@ -228,4 +232,5 @@ private: int flush_uart_rx(); int check_for_esc_timeout(); void mix_turtle_mode(uint16_t outputs[]); + void handle_actuator_test(); }; diff --git a/src/drivers/actuators/modalai_esc/modalai_esc_params.c b/src/drivers/actuators/modalai_esc/modalai_esc_params.c index be1b3ecae4..07ec6e7a60 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc_params.c +++ b/src/drivers/actuators/modalai_esc/modalai_esc_params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0); PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); /** - * Motor mappings for ModalAI ESC M004 + * Motor mappings for ModalAI ESC * * HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed) * 4(1) 3(4) @@ -65,41 +65,16 @@ PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); * 1(3) 2(2) */ -/** - * UART ESC Motor 1 Mapping. 1-4 (negative for reversal). - * - * @group UART ESC - * @min -4 - * @max 4 - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3); +// The following are auto generated params from control allocator pattern, put here for reference -/** - *UART ESC Motor 2 Mapping. 1-4 (negative for reversal). - * - * @group UART ESC - * @min -4 - * @max 4 - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); +// Default ESC1 to motor2 +//PARAM_DEFINE_INT32(UART_ESC_FUNC1, 102); -/** - * UART ESC Motor 3 Mapping. 1-4 (negative for reversal). - * - * @group UART ESC - * @min -4 - * @max 4 - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4); +//PARAM_DEFINE_INT32(UART_ESC_FUNC2, 103); -/** - * UART ESC Motor 4 Mapping. 1-4 (negative for reversal). - * - * @group UART ESC - * @min -4 - * @max 4 - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1); +//PARAM_DEFINE_INT32(UART_ESC_FUNC3, 101); + +//PARAM_DEFINE_INT32(UART_ESC_FUNC4, 104); /** * UART ESC RPM Min @@ -136,6 +111,42 @@ PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000); */ PARAM_DEFINE_INT32(UART_ESC_MODE, 0); +/** + * UART ESC ID 1 Spin Direction Flag + * + * @group UART ESC + * @value 0 - Default + * @value 1 - Reverse + */ +PARAM_DEFINE_INT32(UART_ESC_SDIR1, 0); + +/** + * UART ESC ID 2 Spin Direction Flag + * + * @group UART ESC + * @value 0 - Default + * @value 1 - Reverse + */ +PARAM_DEFINE_INT32(UART_ESC_SDIR2, 0); + +/** + * UART ESC ID 3 Spin Direction Flag + * + * @group UART ESC + * @value 0 - Default + * @value 1 - Reverse + */ +PARAM_DEFINE_INT32(UART_ESC_SDIR3, 0); + +/** + * UART ESC ID 4 Spin Direction Flag + * + * @group UART ESC + * @value 0 - Default + * @value 1 - Reverse + */ +PARAM_DEFINE_INT32(UART_ESC_SDIR4, 0); + /** * UART ESC Turtle Mode Crash Flip Motor Percent * diff --git a/src/drivers/actuators/modalai_esc/module.yaml b/src/drivers/actuators/modalai_esc/module.yaml new file mode 100644 index 0000000000..8019c730e7 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/module.yaml @@ -0,0 +1,26 @@ +module_name: MODALAI ESC Output +actuator_output: + show_subgroups_if: 'UART_ESC_CONFIG>0' + config_parameters: + - param: 'UART_ESC_CONFIG' + label: 'Configure' + function: 'enable' + - param: 'UART_ESC_BAUD' + label: 'Bitrate' + - param: 'UART_ESC_RPM_MIN' + label: 'RPM Min' + - param: 'UART_ESC_RPM_MAX' + label: 'RPM Max' + - param: 'UART_ESC_SDIR1' + label: 'ESC 1 Spin Direction' + - param: 'UART_ESC_SDIR2' + label: 'ESC 2 Spin Direction' + - param: 'UART_ESC_SDIR3' + label: 'ESC 3 Spin Direction' + - param: 'UART_ESC_SDIR4' + label: 'ESC 4 Spin Direction' + output_groups: + - param_prefix: UART_ESC + group_label: 'ESCs' + channel_label: 'ESC' + num_channels: 4