diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 26995f129..bd0062a57 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -88,6 +88,26 @@ config EXAMPLES_FOC_SENSORED endchoice # +if EXAMPLES_FOC_SENSORED + +config EXAMPLES_FOC_HAVE_HALL + bool "FOC example Hall sensor support" + select INDUSTRY_FOC_ANGLE_HALL + default n + +if EXAMPLES_FOC_HAVE_HALL + +config EXAMPLES_FOC_HALL_DEVPATH + string "FOC example Hall sensor path prefix" + default "/dev/hall" + ---help--- + The default path to the Hall device without the device minor number. + Default: /dev/hall + +endif # EXAMPLES_FOC_HAVE_HALL + +endif # EXAMPLES_FOC_SENSORED + config EXAMPLES_FOC_HAVE_OPENLOOP bool "FOC example have open-loop controller" select INDUSTRY_FOC_ANGLE_OPENLOOP diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index 138eef861..880349911 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -31,10 +31,12 @@ * Pre-processor Definitions ****************************************************************************/ -/* For now only open-loop supported */ +/* For now only torque mode supported for sensored */ -#ifndef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP -# error For now only open-loop supported +#ifdef CONFIG_EXAMPLES_FOC_SENSORED +# ifndef CONFIG_EXAMPLES_FOC_HAVE_TORQ +# error +# endif #endif /* For now only sensorless velocity control supported */ diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index 1f67d205e..e43bca27e 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -566,21 +566,19 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor) DEBUGASSERT(motor); #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP -# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL /* Open-loop works only in velocity control mode */ if (motor->openloop_now == true) { +# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL if (motor->envp->mmode != FOC_MMODE_VEL) +#endif { PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n"); ret = -EINVAL; goto errout; } } -# else -# error -# endif #endif /* Get previous DQ */ @@ -670,6 +668,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP struct foc_openloop_cfg_b16_s ol_cfg; #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + struct foc_hall_cfg_b16_s hl_cfg; +#endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN struct foc_routine_align_cfg_b16_s align_cfg; #endif @@ -703,6 +704,37 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, foc_angle_cfg_b16(&motor->openloop, &ol_cfg); #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + /* Initialize hall angle handler */ + + ret = foc_angle_init_b16(&motor->hall, + &g_foc_angle_hl_b16); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret); + goto errout; + } + + /* Get hall devpath */ + + sprintf(motor->hldpath, + "%s%d", + CONFIG_EXAMPLES_FOC_HALL_DEVPATH, + motor->envp->id); + + /* Configure hall angle handler */ + + hl_cfg.devpath = motor->hldpath; + hl_cfg.per = motor->per; + + ret = foc_angle_cfg_b16(&motor->hall, &hl_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Initialize motor alignment routine */ @@ -724,9 +756,11 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, align_cfg.cb.zero = foc_align_zero_cb; align_cfg.cb.dir = foc_align_dir_cb; - /* TODO: Connect align callbacks private data */ + /* Connect align callbacks private data */ - align_cfg.cb.priv = NULL; +# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + align_cfg.cb.priv = &motor->hall; +# endif ret = foc_routine_cfg_b16(&motor->align, &align_cfg); if (ret < 0) @@ -740,7 +774,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, motor->ctrl_state = FOC_CTRL_STATE_INIT; +#ifdef CONFIG_EXAMPLES_FOC_SENSORED errout: +#endif return ret; } @@ -810,6 +846,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) motor->angle_ol = aout.angle; #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + ret = foc_angle_run_b16(&motor->hall, &ain, &aout); + if (ret < 0) + { + PRINTF("ERROR: foc_angle_run failed %d\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_SENSORED /* Handle angle from sensor */ @@ -861,7 +906,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) /* TODO: velocity observer or sensor */ } +#ifdef CONFIG_EXAMPLES_FOC_SENSORED errout: +#endif return ret; } diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index aae0920c4..84d708a30 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -70,6 +70,10 @@ struct foc_motor_b16_s bool openloop_now; /* Open-loop now */ b16_t angle_ol; /* Phase angle open-loop */ foc_angle_b16_t openloop; /* Open-loop angle handler */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + char hldpath[32]; /* Hall devpath */ + foc_angle_b16_t hall; /* Hall angle handler */ #endif int foc_mode; /* FOC mode */ int ctrl_state; /* Controller state */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 54953384e..c8eb3f3ee 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -552,21 +552,19 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor) DEBUGASSERT(motor); #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP -# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL /* Open-loop works only in velocity control mode */ if (motor->openloop_now == true) { +# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL if (motor->envp->mmode != FOC_MMODE_VEL) +# endif { PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n"); ret = -EINVAL; goto errout; } } -# else -# error -# endif #endif /* Get previous DQ */ @@ -656,6 +654,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP struct foc_openloop_cfg_f32_s ol_cfg; #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + struct foc_hall_cfg_f32_s hl_cfg; +#endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN struct foc_routine_align_cfg_f32_s align_cfg; #endif @@ -689,6 +690,37 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, foc_angle_cfg_f32(&motor->openloop, &ol_cfg); #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + /* Initialize hall angle handler */ + + ret = foc_angle_init_f32(&motor->hall, + &g_foc_angle_hl_f32); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret); + goto errout; + } + + /* Get hall devpath */ + + sprintf(motor->hldpath, + "%s%d", + CONFIG_EXAMPLES_FOC_HALL_DEVPATH, + motor->envp->id); + + /* Configure hall angle handler */ + + hl_cfg.devpath = motor->hldpath; + hl_cfg.per = motor->per; + + ret = foc_angle_cfg_f32(&motor->hall, &hl_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Initialize motor alignment routine */ @@ -710,9 +742,11 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, align_cfg.cb.zero = foc_align_zero_cb; align_cfg.cb.dir = foc_align_dir_cb; - /* TODO: Connect align callbacks private data */ + /* Connect align callbacks private data */ - align_cfg.cb.priv = NULL; +# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + align_cfg.cb.priv = &motor->hall; +# endif ret = foc_routine_cfg_f32(&motor->align, &align_cfg); if (ret < 0) @@ -726,7 +760,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, motor->ctrl_state = FOC_CTRL_STATE_INIT; +#ifdef CONFIG_EXAMPLES_FOC_SENSORED errout: +#endif return ret; } @@ -807,6 +843,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) motor->angle_ol = aout.angle; #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + ret = foc_angle_run_f32(&motor->hall, &ain, &aout); + if (ret < 0) + { + PRINTF("ERROR: foc_angle_run failed %d\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_SENSORED /* Handle angle from sensor */ @@ -858,7 +903,9 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) /* TODO: velocity observer or sensor */ } +#ifdef CONFIG_EXAMPLES_FOC_SENSORED errout: +#endif return ret; } diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 80f2fa96f..f69e41a73 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -70,6 +70,10 @@ struct foc_motor_f32_s bool openloop_now; /* Open-loop now */ float angle_ol; /* Phase angle open-loop */ foc_angle_f32_t openloop; /* Open-loop angle handler */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL + char hldpath[32]; /* Hall devpath */ + foc_angle_f32_t hall; /* Hall angle handler */ #endif int foc_mode; /* FOC mode */ int ctrl_state; /* Controller state */