From 95b1625a6d8981f1837ff41ad679f2023809f594 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Wed, 16 Feb 2022 17:43:58 +0100 Subject: [PATCH] examples/foc/foc_motor: store the configured FOC run mode in a separate variable The controller mode can change during example execution and depends on the thread state (IDENT [not yet upstream] / ALIGN / RUN) --- examples/foc/foc_motor_b16.c | 50 ++++++++++++++++++++++++++++++------ examples/foc/foc_motor_b16.h | 3 +++ examples/foc/foc_motor_f32.c | 48 +++++++++++++++++++++++++++++----- examples/foc/foc_motor_f32.h | 3 +++ 4 files changed, 89 insertions(+), 15 deletions(-) diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index d0c7322a9..a734001e6 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -145,19 +145,19 @@ static int foc_runmode_init(FAR struct foc_motor_b16_s *motor) { case FOC_FMODE_IDLE: { - motor->foc_mode = FOC_HANDLER_MODE_IDLE; + motor->foc_mode_run = FOC_HANDLER_MODE_IDLE; break; } case FOC_FMODE_VOLTAGE: { - motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE; + motor->foc_mode_run = FOC_HANDLER_MODE_VOLTAGE; break; } case FOC_FMODE_CURRENT: { - motor->foc_mode = FOC_HANDLER_MODE_CURRENT; + motor->foc_mode_run = FOC_HANDLER_MODE_CURRENT; break; } @@ -169,7 +169,7 @@ static int foc_runmode_init(FAR struct foc_motor_b16_s *motor) } } - /* Force open-loop if sensorlesss */ + /* Force open-loop if sensorless */ #ifdef CONFIG_EXAMPLES_FOC_SENSORLESS # ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP @@ -585,6 +585,18 @@ errout: } #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + +/**************************************************************************** + * Name: foc_motor_run_init + ****************************************************************************/ + +static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor) +{ + /* Empty for now */ + + return OK; +} + /**************************************************************************** * Name: foc_motor_run ****************************************************************************/ @@ -728,6 +740,21 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); +#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + /* Initialize controller run mode */ + + ret = foc_runmode_init(motor); + if (ret < 0) + { + PRINTF("ERROR: foc_runmode_init failed %d!\n", ret); + goto errout; + } +#endif + + /* Start with FOC IDLE mode */ + + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Initialize open-loop angle handler */ @@ -844,7 +871,7 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, motor->ctrl_state = FOC_CTRL_STATE_INIT; -#ifdef CONFIG_EXAMPLES_FOC_SENSORED +#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_HAVE_RUN) errout: #endif return ret; @@ -1050,22 +1077,29 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor) #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN case FOC_CTRL_STATE_RUN_INIT: { - /* Initialize run controller mode */ + /* Initialize controller run mode */ - ret = foc_runmode_init(motor); + ret = foc_motor_run_init(motor); if (ret < 0) { - PRINTF("ERROR: foc_runmode_init failed %d!\n", ret); + PRINTF("ERROR: foc_motor_run_init failed %d!\n", ret); goto errout; } /* Next state */ motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + + break; } case FOC_CTRL_STATE_RUN: { + /* Get FOC run mode */ + + motor->foc_mode = motor->foc_mode_run; + /* Run motor */ ret = foc_motor_run(motor); diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index ea7f2892b..f368e2382 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -80,6 +80,9 @@ struct foc_motor_b16_s foc_angle_b16_t qenco; /* Qenco angle handler */ #endif int foc_mode; /* FOC mode */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + int foc_mode_run; /* FOC mode for run state */ +#endif int ctrl_state; /* Controller state */ b16_t vbus; /* Power bus voltage */ b16_t angle_now; /* Phase angle now */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index cc32d7d7b..a7adf5363 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -145,19 +145,19 @@ static int foc_runmode_init(FAR struct foc_motor_f32_s *motor) { case FOC_FMODE_IDLE: { - motor->foc_mode = FOC_HANDLER_MODE_IDLE; + motor->foc_mode_run = FOC_HANDLER_MODE_IDLE; break; } case FOC_FMODE_VOLTAGE: { - motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE; + motor->foc_mode_run = FOC_HANDLER_MODE_VOLTAGE; break; } case FOC_FMODE_CURRENT: { - motor->foc_mode = FOC_HANDLER_MODE_CURRENT; + motor->foc_mode_run = FOC_HANDLER_MODE_CURRENT; break; } @@ -569,6 +569,18 @@ errout: } #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + +/**************************************************************************** + * Name: foc_motor_run_init + ****************************************************************************/ + +static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor) +{ + /* Empty for now */ + + return OK; +} + /**************************************************************************** * Name: foc_motor_run ****************************************************************************/ @@ -712,6 +724,21 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); +#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + /* Initialize controller run mode */ + + ret = foc_runmode_init(motor); + if (ret < 0) + { + PRINTF("ERROR: foc_runmode_init failed %d!\n", ret); + goto errout; + } +#endif + + /* Start with FOC IDLE mode */ + + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Initialize open-loop angle handler */ @@ -828,7 +855,7 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, motor->ctrl_state = FOC_CTRL_STATE_INIT; -#ifdef CONFIG_EXAMPLES_FOC_SENSORED +#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_HAVE_RUN) errout: #endif return ret; @@ -1045,22 +1072,29 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor) #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN case FOC_CTRL_STATE_RUN_INIT: { - /* Initialize run controller mode */ + /* Initialize controller run mode */ - ret = foc_runmode_init(motor); + ret = foc_motor_run_init(motor); if (ret < 0) { - PRINTF("ERROR: foc_runmode_init failed %d!\n", ret); + PRINTF("ERROR: foc_motor_run_init failed %d!\n", ret); goto errout; } /* Next state */ motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + + break; } case FOC_CTRL_STATE_RUN: { + /* Get FOC run mode */ + + motor->foc_mode = motor->foc_mode_run; + /* Run motor */ ret = foc_motor_run(motor); diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 625c5220e..6bab5119a 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -80,6 +80,9 @@ struct foc_motor_f32_s foc_angle_f32_t qenco; /* Qenco angle handler */ #endif int foc_mode; /* FOC mode */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN + int foc_mode_run; /* FOC mode for run state */ +#endif int ctrl_state; /* Controller state */ float vbus; /* Power bus voltage */ float angle_now; /* Phase angle now */