diff --git a/src/drivers/actuators/voxl_esc/module.yaml b/src/drivers/actuators/voxl_esc/module.yaml index c3bb1f4e6e..1a7e3fa319 100644 --- a/src/drivers/actuators/voxl_esc/module.yaml +++ b/src/drivers/actuators/voxl_esc/module.yaml @@ -19,6 +19,10 @@ actuator_output: label: 'ESC 3 Spin Direction' - param: 'VOXL_ESC_SDIR4' label: 'ESC 4 Spin Direction' + - param: 'VOXL_ESC_CMD' + label: 'ESC Command Type' + - param: 'VOXL_ESC_PWR_MIN' + label: 'ESC Minimum Power With PWM Command Type' output_groups: - param_prefix: VOXL_ESC group_label: 'ESCs' diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 099a552a26..3414851b4b 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -300,6 +300,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_SDIR3"), ¶ms->direction_map[2]); param_get(param_find("VOXL_ESC_SDIR4"), ¶ms->direction_map[3]); + param_get(param_find("VOXL_ESC_PWR_MIN"), ¶ms->pwr_min); param_get(param_find("VOXL_ESC_RPM_MIN"), ¶ms->rpm_min); param_get(param_find("VOXL_ESC_RPM_MAX"), ¶ms->rpm_max); @@ -311,10 +312,23 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_GPIO_CH"), ¶ms->gpio_ctl_channel); - if (params->rpm_min >= params->rpm_max) { - PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); - params->rpm_min = 0; - ret = PX4_ERROR; + param_get(param_find("VOXL_ESC_CMD"), ¶ms->cmd_type); + + if (params->cmd_type > VOXL_ESC_PWM_CMDS) { + PX4_WARN("Warning, VOXL_ESC_CMD set to invalid value %d. Using 1 instead", (int) params->cmd_type); + params->cmd_type = VOXL_ESC_PWM_CMDS; + + } else if (params->cmd_type < VOXL_ESC_RPM_CMDS) { + PX4_WARN("Warning, VOXL_ESC_CMD set to invalid value %d. Using 0 instead", (int) params->cmd_type); + params->cmd_type = VOXL_ESC_RPM_CMDS; + } + + if (params->cmd_type == VOXL_ESC_RPM_CMDS) { + if (params->rpm_min >= params->rpm_max) { + PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); + params->rpm_min = 0; + ret = PX4_ERROR; + } } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { @@ -392,19 +406,24 @@ int VoxlEsc::task_spawn(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "dv", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'd': - _device = argv[myoptind]; - break; + VoxlEsc *instance = new VoxlEsc(); - default: - break; + // Parse any passed in options + if ((argc > 1) && (argv[1] != nullptr)) { + while ((ch = px4_getopt(argc - 1, &argv[1], "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _device = argv[myoptind]; + PX4_INFO("Configuring device as %s", _device); + break; + + default: + PX4_ERR("Unknown option: %c", ch); + break; + } } } - VoxlEsc *instance = new VoxlEsc(); - if (instance) { _object.store(instance); _task_id = task_id_is_work_queue; @@ -571,6 +590,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) QC_ESC_FB_POWER_STATUS packet; memcpy(&packet, _fb_packet.buffer, packet_size); + _rx_power_status_count++; + float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution float current = packet.current * 0.008f; // Total current is reported at 8mA resolution @@ -907,8 +928,19 @@ int VoxlEsc::update_params() if (ret == PX4_OK) { _mixing_output.setAllDisarmedValues(0); _mixing_output.setAllFailsafeValues(0); - _mixing_output.setAllMinValues(_parameters.rpm_min); - _mixing_output.setAllMaxValues(_parameters.rpm_max); + + if (_parameters.cmd_type == VOXL_ESC_RPM_CMDS) { + _mixing_output.setAllMinValues(_parameters.rpm_min); + _mixing_output.setAllMaxValues(_parameters.rpm_max); + + } else if (_parameters.cmd_type == VOXL_ESC_PWM_CMDS) { + // we use a minimum value of 1, since 0 is for disarmed + _min_active_pwm = math::constrain(static_cast((_parameters.pwr_min * + static_cast(VOXL_ESC_PWM_MAX))), + VOXL_ESC_PWM_MIN, VOXL_ESC_PWM_MAX); + _mixing_output.setAllMinValues(_min_active_pwm); + _mixing_output.setAllMaxValues(VOXL_ESC_PWM_MAX); + } _rpm_fullscale = _parameters.rpm_max - _parameters.rpm_min; } @@ -1216,11 +1248,18 @@ bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS], _esc_chans[i].rate_req = 0; } else { - if (_extended_rpm) { - if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; } + if ((_turtle_mode_en) || (_parameters.cmd_type == VOXL_ESC_RPM_CMDS)) { + if (_extended_rpm) { + if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; } - } else { - if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; } + } else { + if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; } + } + + } else if (_parameters.cmd_type == VOXL_ESC_PWM_CMDS) { + if (outputs[i] > VOXL_ESC_PWM_MAX) { outputs[i] = VOXL_ESC_PWM_MAX; } + + else if (outputs[i] < _min_active_pwm) { outputs[i] = _min_active_pwm; } } if (!_turtle_mode_en) { @@ -1235,18 +1274,34 @@ bool VoxlEsc::updateOutputs(uint16_t outputs[MAX_ACTUATORS], Command cmd; - cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, - _esc_chans[1].rate_req, - _esc_chans[2].rate_req, - _esc_chans[3].rate_req, - _esc_chans[0].led, - _esc_chans[1].led, - _esc_chans[2].led, - _esc_chans[3].led, - _fb_idx, - cmd.buf, - sizeof(cmd.buf), - _extended_rpm); + + if (_parameters.cmd_type == VOXL_ESC_RPM_CMDS) { + cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, + _esc_chans[1].rate_req, + _esc_chans[2].rate_req, + _esc_chans[3].rate_req, + _esc_chans[0].led, + _esc_chans[1].led, + _esc_chans[2].led, + _esc_chans[3].led, + _fb_idx, + cmd.buf, + sizeof(cmd.buf), + _extended_rpm); + + } else if (_parameters.cmd_type == VOXL_ESC_PWM_CMDS) { + cmd.len = qc_esc_create_pwm_packet4_fb(_esc_chans[0].rate_req, + _esc_chans[1].rate_req, + _esc_chans[2].rate_req, + _esc_chans[3].rate_req, + _esc_chans[0].led, + _esc_chans[1].led, + _esc_chans[2].led, + _esc_chans[3].led, + _fb_idx, + cmd.buf, + sizeof(cmd.buf)); + } if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { PX4_ERR("Failed to send packet"); @@ -1631,6 +1686,8 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_SDIR3: %" PRId32, _parameters.direction_map[2]); PX4_INFO("Params: VOXL_ESC_SDIR4: %" PRId32, _parameters.direction_map[3]); + PX4_INFO("Params: VOXL_ESC_PWR_MIN: %f", (double) _parameters.pwr_min); + PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min); PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max); @@ -1647,6 +1704,8 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold); PX4_INFO("Params: VOXL_ESC_GPIO_CH: %" PRId32, _parameters.gpio_ctl_channel); + + PX4_INFO("Params: VOXL_ESC_CMD: %" PRId32, _parameters.cmd_type); } int VoxlEsc::print_status() @@ -1656,6 +1715,10 @@ int VoxlEsc::print_status() PX4_INFO("UART port: %s", _device); PX4_INFO("UART open: %s", _uart_port.isOpen() ? "yes" : "no"); + PX4_INFO("CRC error count: %lu", (long unsigned int) _rx_crc_error_count); + PX4_INFO("Packet RX count: %lu", (long unsigned int) _rx_packet_count); + PX4_INFO("Power status count: %lu", (long unsigned int) _rx_power_status_count); + PX4_INFO(""); print_params(); PX4_INFO(""); @@ -1695,6 +1758,7 @@ const char * VoxlEsc::board_id_to_name(int board_id) case 40: return "ModalAi 4-in-1 ESC (M0129-3)"; case 41: return "ModalAi 4-in-1 ESC (M0134-6)"; case 42: return "ModalAi 4-in-1 ESC (M0138-1)"; + case 44: return "ModalAi 4-in-1 ESC (M0129-6)"; default: return "Unknown Board"; } } diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index ee8b931f07..d103f902d0 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -117,8 +117,8 @@ private: static constexpr uint16_t DISARMED_VALUE = 0; - static constexpr uint16_t VOXL_ESC_PWM_MIN = 0; - static constexpr uint16_t VOXL_ESC_PWM_MAX = 800; + static constexpr int VOXL_ESC_PWM_MIN = 1; + static constexpr int VOXL_ESC_PWM_MAX = 800; static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MIN = 5000; static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MAX = 17000; @@ -148,6 +148,9 @@ private: static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX5 = 5; static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX6 = 6; + static constexpr int32_t VOXL_ESC_RPM_CMDS = 0; + static constexpr int32_t VOXL_ESC_PWM_CMDS = 1; + Serial _uart_port{}; typedef struct { @@ -159,6 +162,7 @@ private: float turtle_stick_minf{0.15f}; float turtle_cosphi{0.99f}; int32_t baud_rate{VOXL_ESC_DEFAULT_BAUD}; + float pwr_min{0.05f}; int32_t rpm_min{VOXL_ESC_DEFAULT_RPM_MIN}; int32_t rpm_max{VOXL_ESC_DEFAULT_RPM_MAX}; int32_t function_map[VOXL_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0}; @@ -169,6 +173,7 @@ private: int32_t esc_warn_temp_threshold{0}; int32_t esc_over_temp_threshold{0}; int32_t gpio_ctl_channel{0}; + int32_t cmd_type{0}; } voxl_esc_params_t; struct EscChan { @@ -222,6 +227,8 @@ private: bool _need_version_info{true}; QC_ESC_EXTENDED_VERSION_INFO _version_info[VOXL_ESC_OUTPUT_CHANNELS]; + int _min_active_pwm{1}; + voxl_esc_params_t _parameters; int update_params(); int load_params(voxl_esc_params_t *params, ch_assign_t *map); @@ -250,12 +257,13 @@ private: int _fb_idx; uint32_t _rx_crc_error_count{0}; uint32_t _rx_packet_count{0}; + uint32_t _rx_power_status_count{0}; static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; Battery _battery; - static constexpr unsigned _battery_report_interval{100_ms}; + static constexpr unsigned _battery_report_interval{20_ms}; hrt_abstime _last_battery_report_time; hrt_abstime _last_uart_passthru{0}; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index b0526ecf1f..6ccfeb60ba 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -276,3 +276,31 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0); * @max 6 */ PARAM_DEFINE_INT32(VOXL_ESC_GPIO_CH, 0); + +/** + * UART ESC command type + * + * Selects between RPM or PWM commands + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - RPM + * @value 1 - PWM + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(VOXL_ESC_CMD, 0); + +/** + * UART ESC Minimum motor power + * + * Minimum motor power for ESC when VOXL_ESC_CMD is set for PWM. + * Make sure to set this high enough so that the motors are always spinning + * while armed. + * + * @group VOXL ESC + * @min 0.0 + * @max 1.0 + */ +PARAM_DEFINE_FLOAT(VOXL_ESC_PWR_MIN, 0.05);