Skip to content

Apni2/tmc51xx log progress #88903

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Apr 24, 2025
Merged

Apni2/tmc51xx log progress #88903

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dd7cd56
drivers: stepper: adi_tmc: Use helper functions in tmc50xx.
apni2 Apr 22, 2025
508e7c9
drivers: stepper: adi_tmc: Add progress log for stall guard tuning.
apni2 Apr 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 29 additions & 20 deletions drivers/stepper/adi_tmc/adi_tmc50xx_stepper_controller.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,23 @@ static int tmc50xx_stepper_set_event_callback(const struct device *dev,
return 0;
}

static int read_vactual(const struct tmc50xx_stepper_config *config, int32_t *actual_velocity)
{
int err;

err = tmc50xx_read(config->controller, TMC50XX_VACTUAL(config->index), actual_velocity);
if (err) {
LOG_ERR("Failed to read VACTUAL register");
return err;
}

*actual_velocity = sign_extend(*actual_velocity, TMC_RAMP_VACTUAL_SHIFT);
if (actual_velocity) {
LOG_DBG("actual velocity: %d", *actual_velocity);
}
return 0;
}

static int stallguard_enable(const struct device *dev, const bool enable)
{
const struct tmc50xx_stepper_config *config = dev->config;
Expand All @@ -122,17 +139,10 @@ static int stallguard_enable(const struct device *dev, const bool enable)

int32_t actual_velocity;

err = tmc50xx_read(config->controller, TMC50XX_VACTUAL(config->index),
&actual_velocity);
err = read_vactual(config, &actual_velocity);
if (err) {
LOG_ERR("Failed to read VACTUAL register");
return -EIO;
}

actual_velocity = (actual_velocity << (31 - TMC_RAMP_VACTUAL_SHIFT)) >>
(31 - TMC_RAMP_VACTUAL_SHIFT);
LOG_DBG("actual velocity: %d", actual_velocity);

if (abs(actual_velocity) < config->sg_threshold_velocity) {
return -EAGAIN;
}
Expand Down Expand Up @@ -197,12 +207,18 @@ static void log_stallguard(struct tmc50xx_stepper_data *stepper_data, const uint
const uint8_t sg_result = FIELD_GET(TMC5XXX_DRV_STATUS_SG_RESULT_MASK, drv_status);
const bool sg_status = FIELD_GET(TMC5XXX_DRV_STATUS_SG_STATUS_MASK, drv_status);

LOG_DBG("%s position: %d | sg result: %d status: %d",
LOG_DBG("%s position: %d | sg result: %3d status: %d",
stepper_data->stepper->name, position, sg_result, sg_status);
}

#endif

static void rampstat_work_reschedule(struct k_work_delayable *rampstat_callback_dwork)
{
k_work_reschedule(rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
}

static void rampstat_work_handler(struct k_work *work)
{
struct k_work_delayable *dwork = k_work_delayable_from_work(work);
Expand Down Expand Up @@ -277,9 +293,7 @@ static void rampstat_work_handler(struct k_work *work)
break;
}
} else {
k_work_reschedule(
&stepper_data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&stepper_data->rampstat_callback_dwork);
}
}

Expand Down Expand Up @@ -474,9 +488,7 @@ static int tmc50xx_stepper_move_to(const struct device *dev, const int32_t micro
}
#ifdef CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL
if (data->callback) {
k_work_reschedule(
&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&data->rampstat_callback_dwork);
}
#endif
return 0;
Expand Down Expand Up @@ -536,9 +548,7 @@ static int tmc50xx_stepper_run(const struct device *dev, const enum stepper_dire
}
#ifdef CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL
if (data->callback) {
k_work_reschedule(
&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&data->rampstat_callback_dwork);
}
#endif
return 0;
Expand Down Expand Up @@ -687,8 +697,7 @@ static int tmc50xx_stepper_init(const struct device *dev)

#if CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL
k_work_init_delayable(&data->rampstat_callback_dwork, rampstat_work_handler);
k_work_reschedule(&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC50XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&data->rampstat_callback_dwork);
#endif
err = tmc50xx_stepper_set_micro_step_res(dev, stepper_config->default_micro_step_res);
if (err != 0) {
Expand Down
118 changes: 86 additions & 32 deletions drivers/stepper/adi_tmc/adi_tmc51xx_stepper_controller.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ struct tmc51xx_config {
#endif
};

static int read_actual_position(const struct device *dev, int32_t *position);

static int tmc51xx_write(const struct device *dev, const uint8_t reg_addr, const uint32_t reg_val)
{
const struct tmc51xx_config *config = dev->config;
Expand All @@ -57,7 +59,7 @@ static int tmc51xx_write(const struct device *dev, const uint8_t reg_addr, const

k_sem_give(&data->sem);

if (err) {
if (err < 0) {
LOG_ERR("Failed to write register 0x%x with value 0x%x", reg_addr, reg_val);
return err;
}
Expand All @@ -77,7 +79,7 @@ static int tmc51xx_read(const struct device *dev, const uint8_t reg_addr, uint32

k_sem_give(&data->sem);

if (err) {
if (err < 0) {
LOG_ERR("Failed to read register 0x%x", reg_addr);
return err;
}
Expand All @@ -94,6 +96,23 @@ static int tmc51xx_stepper_set_event_callback(const struct device *dev,
return 0;
}

static int read_vactual(const struct device *dev, int32_t *actual_velocity)
{
int err;

err = tmc51xx_read(dev, TMC51XX_VACTUAL, actual_velocity);
if (err) {
LOG_ERR("Failed to read VACTUAL register");
return err;
}

*actual_velocity = sign_extend(*actual_velocity, TMC_RAMP_VACTUAL_SHIFT);
if (*actual_velocity) {
LOG_DBG("actual velocity: %d", *actual_velocity);
}
return 0;
}

static int stallguard_enable(const struct device *dev, const bool enable)
{
const struct tmc51xx_config *config = dev->config;
Expand All @@ -111,17 +130,10 @@ static int stallguard_enable(const struct device *dev, const bool enable)

int32_t actual_velocity;

err = tmc51xx_read(dev, TMC51XX_VACTUAL,
&actual_velocity);
err = read_vactual(dev, &actual_velocity);
if (err) {
LOG_ERR("Failed to read VACTUAL register");
return -EIO;
}

actual_velocity = (actual_velocity << (31 - TMC_RAMP_VACTUAL_SHIFT)) >>
(31 - TMC_RAMP_VACTUAL_SHIFT);
LOG_DBG("actual velocity: %d", actual_velocity);

if (abs(actual_velocity) < config->sg_threshold_velocity) {
return -EAGAIN;
}
Expand All @@ -133,6 +145,8 @@ static int stallguard_enable(const struct device *dev, const bool enable)
LOG_ERR("Failed to write SWMODE register");
return -EIO;
}

LOG_DBG("Stallguard %s", enable ? "enabled" : "disabled");
return 0;
}

Expand All @@ -147,7 +161,6 @@ static void stallguard_work_handler(struct k_work *work)

err = stallguard_enable(dev, true);
if (err == -EAGAIN) {
LOG_ERR("retrying stallguard activation");
k_work_reschedule(dwork, K_MSEC(config->sg_velocity_check_interval_ms));
}
if (err == -EIO) {
Expand All @@ -169,6 +182,45 @@ static void execute_callback(const struct device *dev, const enum stepper_event
data->callback(dev, event, data->event_cb_user_data);
}

#ifdef CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_STALLGUARD_LOG

static void log_stallguard(const struct device *dev, const uint32_t drv_status)
{
int32_t position;
int err;

err = read_actual_position(dev, &position);
if (err != 0) {
LOG_ERR("%s: Failed to read XACTUAL register", dev->name);
return;
}

const uint8_t sg_result = FIELD_GET(TMC5XXX_DRV_STATUS_SG_RESULT_MASK, drv_status);
const bool sg_status = FIELD_GET(TMC5XXX_DRV_STATUS_SG_STATUS_MASK, drv_status);

LOG_DBG("%s position: %d | sg result: %3d status: %d",
dev->name, position, sg_result, sg_status);
}

#endif

static void rampstat_work_reschedule(struct k_work_delayable *rampstat_callback_dwork)
{
k_work_reschedule(rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
}

static int rampstat_read_clear(const struct device *dev, uint32_t *rampstat_value)
{
int err;

err = tmc51xx_read(dev, TMC51XX_RAMPSTAT, rampstat_value);
if (err == 0) {
err = tmc51xx_write(dev, TMC51XX_RAMPSTAT, *rampstat_value);
}
return err;
}

static void rampstat_work_handler(struct k_work *work)
{
struct k_work_delayable *dwork = k_work_delayable_from_work(work);
Expand All @@ -188,7 +240,9 @@ static void rampstat_work_handler(struct k_work *work)
LOG_ERR("%s: Failed to read DRVSTATUS register", dev->name);
return;
}

#ifdef CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_STALLGUARD_LOG
log_stallguard(dev, drv_status);
#endif
if (FIELD_GET(TMC5XXX_DRV_STATUS_SG_STATUS_MASK, drv_status) == 1U) {
LOG_INF("%s: Stall detected", dev->name);
err = tmc51xx_write(dev,
Expand All @@ -202,8 +256,7 @@ static void rampstat_work_handler(struct k_work *work)

uint32_t rampstat_value;

err = tmc51xx_read(dev, TMC51XX_RAMPSTAT,
&rampstat_value);
err = rampstat_read_clear(dev, &rampstat_value);
if (err != 0) {
LOG_ERR("%s: Failed to read RAMPSTAT register", dev->name);
return;
Expand Down Expand Up @@ -241,9 +294,7 @@ static void rampstat_work_handler(struct k_work *work)
break;
}
} else {
k_work_reschedule(
&stepper_data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&stepper_data->rampstat_callback_dwork);
}
}

Expand Down Expand Up @@ -380,14 +431,25 @@ static int tmc51xx_stepper_set_reference_position(const struct device *dev, cons
return 0;
}

static int tmc51xx_stepper_get_actual_position(const struct device *dev, int32_t *position)
static int read_actual_position(const struct device *dev, int32_t *position)
{
int err;

err = tmc51xx_read(dev, TMC51XX_XACTUAL, position);
if (err != 0) {
return -EIO;
}
return 0;
}

static int tmc51xx_stepper_get_actual_position(const struct device *dev, int32_t *position)
{
int err;

err = read_actual_position(dev, position);
if (err != 0) {
return -EIO;
}
LOG_DBG("%s actual position: %d", dev->name, *position);
return 0;
}
Expand Down Expand Up @@ -419,9 +481,7 @@ static int tmc51xx_stepper_move_to(const struct device *dev, const int32_t micro
}
#ifdef CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL
if (data->callback) {
k_work_reschedule(
&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&data->rampstat_callback_dwork);
}
#endif
return 0;
Expand Down Expand Up @@ -481,9 +541,7 @@ static int tmc51xx_stepper_run(const struct device *dev, const enum stepper_dire
}
#ifdef CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL
if (data->callback) {
k_work_reschedule(
&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
rampstat_work_reschedule(&data->rampstat_callback_dwork);
}
#endif
return 0;
Expand Down Expand Up @@ -617,12 +675,7 @@ static int tmc51xx_init(const struct device *dev)
if (err != 0) {
return -EIO;
}
err = stallguard_enable(dev, true);
if (err == -EAGAIN) {
LOG_ERR("retrying stallguard activation");
k_work_reschedule(&data->stallguard_dwork,
K_MSEC(config->sg_velocity_check_interval_ms));
}
k_work_reschedule(&data->stallguard_dwork, K_NO_WAIT);
}

#ifdef CONFIG_STEPPER_ADI_TMC51XX_RAMP_GEN
Expand All @@ -634,8 +687,9 @@ static int tmc51xx_init(const struct device *dev)

#if CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL
k_work_init_delayable(&data->rampstat_callback_dwork, rampstat_work_handler);
k_work_reschedule(&data->rampstat_callback_dwork,
K_MSEC(CONFIG_STEPPER_ADI_TMC51XX_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
uint32_t rampstat_value;

(void)rampstat_read_clear(dev, &rampstat_value);
#endif
err = tmc51xx_stepper_set_micro_step_res(dev, config->default_micro_step_res);
if (err != 0) {
Expand Down
Loading