Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

VOXL_ESC: bringing the driver up to date with ModalAI latest. Includes new open loop "PWM" command handling.

Browse Source
This commit is contained in:
Eric Katzfey
2026-02-09 15:06:49 -07:00
committed by Eric Katzfey
parent aacb7e35dd
commit 1b7e12cf90
4 changed files with 138 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/voxl_esc/module.yaml
+4
View File
@@ -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'
src/drivers/actuators/voxl_esc/voxl_esc.cpp
+95 -31
View File
@@ -300,6 +300,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
param_get(param_find("VOXL_ESC_SDIR3"), &params->direction_map[2]);
param_get(param_find("VOXL_ESC_SDIR4"), &params->direction_map[3]);
param_get(param_find("VOXL_ESC_PWR_MIN"), &params->pwr_min);
param_get(param_find("VOXL_ESC_RPM_MIN"), &params->rpm_min);
param_get(param_find("VOXL_ESC_RPM_MAX"), &params->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"), &params->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"), &params->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<int>((_parameters.pwr_min *
static_cast<float>(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";
}
}
src/drivers/actuators/voxl_esc/voxl_esc.hpp
+11 -3
View File
@@ -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};
src/drivers/actuators/voxl_esc/voxl_esc_params.c
+28
View File
@@ -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);