From c5314b54a4a015c8ab827e301e5d8ebca1e7443b Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Wed, 8 Nov 2023 15:06:17 +0100 Subject: [PATCH] examples/foc: fix active brake for sensored mode --- examples/foc/foc_motor_b16.c | 25 +++++++++++++++++++++---- examples/foc/foc_motor_f32.c | 25 +++++++++++++++++++++---- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index e277008fc..8bc0f5173 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -624,7 +624,7 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) DEBUGASSERT(motor); - /* Update motor state */ + /* Update motor state - this function is called every controller cycle */ switch (state) { @@ -642,6 +642,9 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) case FOC_EXAMPLE_STATE_STOP: { +#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS + /* For sensorless we can just set Q reference to lock the motor */ + motor->dir = DIR_NONE_B16; /* DQ vector not zero - active brake */ @@ -649,6 +652,15 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) motor->dq_ref.q = ftob16(CONFIG_EXAMPLES_FOC_STOP_CURRENT / 1000.0f); motor->dq_ref.d = 0; +#else + /* For sensored mode we set requested velocity to 0 */ + +# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + motor->vel.des = 0; +# else +# error STOP state for sensored mode requires velocity support +# endif +#endif break; } @@ -899,10 +911,15 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor) q_ref = motor->dq_ref.q; d_ref = motor->dq_ref.d; - /* Ignore controller if motor is free or stopped */ + /* Ignore controller if motor is free (sensorless and sensored mode) + * or stopped (only sensorless mode) + */ - if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE || - motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) + if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE +#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS + || motor->mq.app_state == FOC_EXAMPLE_STATE_STOP +#endif + ) { goto no_controller; } diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index e4a282e47..11fd3d6eb 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -602,7 +602,7 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state) DEBUGASSERT(motor); - /* Update motor state */ + /* Update motor state - this function is called every controller cycle */ switch (state) { @@ -620,12 +620,24 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state) case FOC_EXAMPLE_STATE_STOP: { +#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS + /* For sensorless we can just set Q reference to lock the motor */ + motor->dir = DIR_NONE; /* DQ vector not zero - active brake */ motor->dq_ref.q = CONFIG_EXAMPLES_FOC_STOP_CURRENT / 1000.0f; motor->dq_ref.d = 0.0f; +#else + /* For sensored mode we set requested velocity to 0 */ + +# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + motor->vel.des = 0.0f; +# else +# error STOP state for sensored mode requires velocity support +# endif +#endif break; } @@ -882,10 +894,15 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor) q_ref = motor->dq_ref.q; d_ref = motor->dq_ref.d; - /* Ignore controller if motor is free or stopped */ + /* Ignore controller if motor is free (sensorless and sensored mode) + * or stopped (only sensorless mode) + */ - if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE || - motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) + if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE +#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS + || motor->mq.app_state == FOC_EXAMPLE_STATE_STOP +#endif + ) { goto no_controller; }