Skip to content

Commit

Permalink
Merge pull request #64 from modalai/gpio-ctl
Browse files Browse the repository at this point in the history 
voxl-esc: gpio control
  • Loading branch information
@modalai-tom
modalai-tom authored Jul 16, 2024
2 parents 85d559e + 67b29ec commit f9d249f
Show file tree
Hide file tree
Showing 3 changed files with 144 additions and 16 deletions.
127 changes: 114 additions & 13 deletions src/drivers/actuators/voxl_esc/voxl_esc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ int VoxlEsc::init()
//https://cwiki.apache.org/confluence/display/NUTTX/Detaching+File+Descriptors
//detaching file descriptors is not implemented in the current version of nuttx that px4 uses
//
//There is no problem when running on VOXL2, but in order to have the same logical flow on both systems,
//There is no problem when running on VOXL2, but in order to have the same logical flow on both systems,
//we will initialize uart and query the device in Run()

ScheduleNow();
Expand Down Expand Up @@ -199,7 +199,7 @@ int VoxlEsc::device_init()
if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) {
QC_ESC_EXTENDED_VERSION_INFO ver;
memcpy(&ver, _fb_packet.buffer, packet_size);

PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id);
PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version));

Expand Down Expand Up @@ -238,7 +238,7 @@ int VoxlEsc::device_init()
//check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*"
for (int esc_id=1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++){
if (strncmp(_version_info[0].firmware_git_version,_version_info[esc_id].firmware_git_version, 9) != 0) {
PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)",
PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)",
esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version);
esc_detection_fault = true;
}
Expand Down Expand Up @@ -302,10 +302,12 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)

param_get(param_find("VOXL_ESC_VLOG"), &params->verbose_logging);
param_get(param_find("VOXL_ESC_PUB_BST"), &params->publish_battery_status);

param_get(param_find("VOXL_ESC_T_WARN"), &params->esc_warn_temp_threshold);
param_get(param_find("VOXL_ESC_T_OVER"), &params->esc_over_temp_threshold);

param_get(param_find("GPIO_CTL_CH"), &params->gpio_ctl_channel);

if (params->rpm_min >= params->rpm_max) {
PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters.");
params->rpm_min = 0;
Expand Down Expand Up @@ -342,6 +344,12 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
ret = PX4_ERROR;
}

if (params->gpio_ctl_channel < 0 || params->gpio_ctl_channel > 6) {
PX4_ERR("VOXL_ESC: Invalid parameter GPIO_CTL_CH. Please verify parameters.");
params->gpio_ctl_channel = 0;
ret = PX4_ERROR;
}

for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters.");
Expand Down Expand Up @@ -515,19 +523,19 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)

_esc_status.timestamp = _esc_status.esc[id].timestamp;
_esc_status.counter++;


if ((_parameters.esc_over_temp_threshold > 0) && (_esc_status.esc[id].esc_temperature > _parameters.esc_over_temp_threshold))
{
_esc_status.esc[id].failures |= 1<<(esc_report_s::FAILURE_OVER_ESC_TEMPERATURE);
}

//TODO: do we also issue a warning if over-temperature threshold is exceeded?
if ((_parameters.esc_warn_temp_threshold > 0) && (_esc_status.esc[id].esc_temperature > _parameters.esc_warn_temp_threshold))
{
_esc_status.esc[id].failures |= 1<<(esc_report_s::FAILURE_WARN_ESC_TEMPERATURE);
}


//print ESC status just for debugging
/*
Expand All @@ -543,7 +551,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) {
QC_ESC_VERSION_INFO ver;
memcpy(&ver, _fb_packet.buffer, packet_size);

PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id);
PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version);
PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version);
Expand All @@ -565,7 +573,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
} else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) {
QC_ESC_FB_POWER_STATUS packet;
memcpy(&packet,_fb_packet.buffer, packet_size);

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

Expand All @@ -581,7 +589,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_battery.updateAndPublishBatteryStatus(current_time);
}
}

}

} else { //parser error
Expand Down Expand Up @@ -1196,6 +1204,12 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
//in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function)
//So, if Run() is blocked by a custom command, this function will not be called until Run is running again

// counter to track how many times uart_write has been called with gpio data
static int gpio_write_counter = 0;

// store the previous state of _gpio_ctl_high
static bool prev_gpio_ctl_high = _gpio_ctl_high;

if (num_outputs != VOXL_ESC_OUTPUT_CHANNELS) {
return false;
}
Expand Down Expand Up @@ -1224,7 +1238,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
}
}


Command cmd;
cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req,
Expand All @@ -1240,11 +1254,56 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
sizeof(cmd.buf),
_extended_rpm);


if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
PX4_ERR("VOXL_ESC: Failed to send packet");
return false;
}

// Track and manage gpio command writes
bool write_gpio_command = false;

if (_gpio_ctl_en) {
if (_gpio_ctl_high != prev_gpio_ctl_high) {
gpio_write_counter = 0;
}

if (gpio_write_counter < 10) {
write_gpio_command = true;
gpio_write_counter++;
}

prev_gpio_ctl_high = _gpio_ctl_high;

if (write_gpio_command) {
Command gpio_cmd;
const int ESC_PACKET_TYPE_GPIO_CMD = 15;
uint8_t data[5];

int esc_id = 0; // In future un-hardcode
int val = 0;

if ( _gpio_ctl_high ) {
val = 1;
}

data[0] = esc_id; // esc id
data[1] = 80; // 01010000 : pin F0
data[2] = 0; // 0: output, 1: input
data[3] = val; //cmd LSB
data[4] = 0; // cmd MSB

// type, data, size
gpio_cmd.len = qc_esc_create_packet(ESC_PACKET_TYPE_GPIO_CMD, (uint8_t *) & (data[0]), 5, gpio_cmd.buf, sizeof(gpio_cmd.buf));

if (_uart_port->uart_write(gpio_cmd.buf, gpio_cmd.len) != gpio_cmd.len) {
PX4_ERR("VOXL_ESC: Failed to send gpio packet");
return false;
}
}

}

// increment ESC id from which to request feedback in round robin order
_fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS;

Expand Down Expand Up @@ -1399,6 +1458,46 @@ void VoxlEsc::Run()

}

// check if gpio control is enabled
if (_parameters.gpio_ctl_channel > 0) {

if (_manual_control_setpoint_sub.updated()) {

_manual_control_setpoint_sub.copy(&_manual_control_setpoint);

_gpio_ctl_en = true;
float gpio_setpoint = VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT;

switch (_parameters.gpio_ctl_channel) {
case VOXL_ESC_GPIO_CTL_AUX1:
gpio_setpoint = _manual_control_setpoint.aux1;
break;
case VOXL_ESC_GPIO_CTL_AUX2:
gpio_setpoint = _manual_control_setpoint.aux2;
break;
case VOXL_ESC_GPIO_CTL_AUX3:
gpio_setpoint = _manual_control_setpoint.aux3;
break;
case VOXL_ESC_GPIO_CTL_AUX4:
gpio_setpoint = _manual_control_setpoint.aux4;
break;
case VOXL_ESC_GPIO_CTL_AUX5:
gpio_setpoint = _manual_control_setpoint.aux5;
break;
case VOXL_ESC_GPIO_CTL_AUX6:
gpio_setpoint = _manual_control_setpoint.aux6;
break;
}

if (gpio_setpoint > VOXL_ESC_GPIO_CTL_THRESHOLD) {
_gpio_ctl_high = false;

} else {
_gpio_ctl_high = true;
}
}
}

if (!_outputs_on) {
if (_actuator_test_sub.updated()) {
// values are set in ActuatorTest::update, we just need to enable outputs to let them through
Expand Down Expand Up @@ -1544,9 +1643,11 @@ void VoxlEsc::print_params()

PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging);
PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status);

PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold);
PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold);

PX4_INFO("Params: GPIO_CTL_CH: %" PRId32, _parameters.gpio_ctl_channel);
}

int VoxlEsc::print_status()
Expand Down
14 changes: 14 additions & 0 deletions src/drivers/actuators/voxl_esc/voxl_esc.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,16 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface

static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3;

static constexpr float VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT = -0.1f;
static constexpr float VOXL_ESC_GPIO_CTL_THRESHOLD = 0.0f;

static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX1 = 1;
static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX2 = 2;
static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX3 = 3;
static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX4 = 4;
static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX5 = 5;
static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX6 = 6;

//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); }

Expand All @@ -156,6 +166,7 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
int32_t publish_battery_status{0};
int32_t esc_warn_temp_threshold{0};
int32_t esc_over_temp_threshold{0};
int32_t gpio_ctl_channel{0};
} voxl_esc_params_t;

struct EscChan {
Expand Down Expand Up @@ -219,6 +230,9 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
int32_t _rpm_fullscale{0};
manual_control_setpoint_s _manual_control_setpoint{};

bool _gpio_ctl_en{false};
bool _gpio_ctl_high{true};

uint16_t _cmd_id{0};
Command _current_cmd;
px4::atomic<Command *> _pending_cmd{nullptr};
Expand Down
19 changes: 16 additions & 3 deletions src/drivers/actuators/voxl_esc/voxl_esc_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -220,7 +220,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0);

/**
* UART ESC Enable publishing of battery status
*
*
* Only applicable to ESCs that report total battery voltage and current
*
* @reboot_required true
Expand All @@ -236,7 +236,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1);

/**
* UART ESC Temperature Warning Threshold (Degrees C)
*
*
* Only applicable to ESCs that report temperature
*
* @reboot_required true
Expand All @@ -251,7 +251,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0);

/**
* UART ESC Over-Temperature Threshold (Degrees C)
*
*
* Only applicable to ESCs that report temperature
*
* @reboot_required true
Expand All @@ -263,3 +263,16 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0);
*/
PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0);


/**
* GPIO Control Channel
*
*
* @reboot_required true
*
* @group VOXL ESC
* @value 0 - Disabled
* @min 0
* @max 6
*/
PARAM_DEFINE_INT32(GPIO_CTL_CH, 0);

0 comments on commit f9d249f

Please sign in to comment.