From ff839dcf3e2406b122a3278bd103af7589e0bc60 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Fri, 29 Oct 2021 12:42:19 +0200 Subject: [PATCH] industry/foc/*/foc_openloop.c: adapt to previous changes --- industry/foc/fixed16/foc_openloop.c | 97 ++++++++++++++++++++++++++--- industry/foc/float/foc_openloop.c | 90 +++++++++++++++++++++++--- 2 files changed, 169 insertions(+), 18 deletions(-) diff --git a/industry/foc/fixed16/foc_openloop.c b/industry/foc/fixed16/foc_openloop.c index 1a272c948..dd8d4969d 100644 --- a/industry/foc/fixed16/foc_openloop.c +++ b/industry/foc/fixed16/foc_openloop.c @@ -32,7 +32,7 @@ #include "industry/foc/fixed16/foc_angle.h" /**************************************************************************** - * Private Function Prototypes + * Private Data Types ****************************************************************************/ /* Open-loop data */ @@ -41,6 +41,7 @@ struct foc_openloop_b16_s { struct foc_openloop_cfg_b16_s cfg; struct openloop_data_b16_s data; + b16_t dir; }; /**************************************************************************** @@ -50,9 +51,11 @@ struct foc_openloop_b16_s static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h); static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h); static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg); -static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h, - FAR struct foc_angle_in_b16_s *in, - FAR struct foc_angle_out_b16_s *out); +static int foc_angle_ol_zero_b16(FAR foc_angle_b16_t *h); +static int foc_angle_ol_dir_b16(FAR foc_angle_b16_t *h, b16_t dir); +static int foc_angle_ol_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out); /**************************************************************************** * Public Data @@ -63,6 +66,8 @@ struct foc_angle_ops_b16_s g_foc_angle_ol_b16 = .init = foc_angle_ol_init_b16, .deinit = foc_angle_ol_deinit_b16, .cfg = foc_angle_ol_cfg_b16, + .zero = foc_angle_ol_zero_b16, + .dir = foc_angle_ol_dir_b16, .run = foc_angle_ol_run_b16, }; @@ -142,7 +147,7 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg) DEBUGASSERT(h); - /* Get modulation data */ + /* Get open-loop data */ DEBUGASSERT(h->data); ol = h->data; @@ -156,6 +161,72 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg) DEBUGASSERT(ol->cfg.per > 0); motor_openloop_init_b16(&ol->data, ol->cfg.per); + /* Initialize with CW direction */ + + ol->dir = DIR_CW_B16; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_ol_zero_b16 + * + * Description: + * Zero the open-loop FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_ol_zero_b16(FAR foc_angle_b16_t *h) +{ + FAR struct foc_openloop_b16_s *ol = NULL; + + DEBUGASSERT(h); + + /* Get open-loop data */ + + DEBUGASSERT(h->data); + ol = h->data; + + /* Reset angle */ + + ol->data.angle = 0; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_ol_dir_b16 + * + * Description: + * Set the open-loop FOC angle handler direction (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_ol_dir_b16(FAR foc_angle_b16_t *h, b16_t dir) +{ + FAR struct foc_openloop_b16_s *ol = NULL; + + DEBUGASSERT(h); + + UNUSED(dir); + + /* Get open-loop data */ + + DEBUGASSERT(h->data); + ol = h->data; + + /* Configure direction */ + + ol->dir = dir; + return OK; } @@ -172,15 +243,16 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg) * ****************************************************************************/ -static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h, - FAR struct foc_angle_in_b16_s *in, - FAR struct foc_angle_out_b16_s *out) +static int foc_angle_ol_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out) { FAR struct foc_openloop_b16_s *ol = NULL; + b16_t tmp = 0; DEBUGASSERT(h); - /* Get modulation data */ + /* Get open-loop data */ DEBUGASSERT(h->data); ol = h->data; @@ -189,7 +261,12 @@ static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h, motor_openloop_b16(&ol->data, in->vel, in->dir); + tmp = motor_openloop_angle_get_b16(&ol->data); + /* Get open-loop angle */ - out->angle = motor_openloop_angle_get_b16(&ol->data); + out->type = FOC_ANGLE_TYPE_ELE; + out->angle = b16mulb16(ol->dir, tmp); + + return OK; } diff --git a/industry/foc/float/foc_openloop.c b/industry/foc/float/foc_openloop.c index 365b0be4c..e4a26ad58 100644 --- a/industry/foc/float/foc_openloop.c +++ b/industry/foc/float/foc_openloop.c @@ -32,7 +32,7 @@ #include "industry/foc/float/foc_angle.h" /**************************************************************************** - * Private Function Prototypes + * Private Data Types ****************************************************************************/ /* Open-loop data */ @@ -41,6 +41,7 @@ struct foc_openloop_f32_s { struct foc_openloop_cfg_f32_s cfg; struct openloop_data_f32_s data; + float dir; }; /**************************************************************************** @@ -50,9 +51,11 @@ struct foc_openloop_f32_s static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h); static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h); static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg); -static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, - FAR struct foc_angle_in_f32_s *in, - FAR struct foc_angle_out_f32_s *out); +static int foc_angle_ol_zero_f32(FAR foc_angle_f32_t *h); +static int foc_angle_ol_dir_f32(FAR foc_angle_f32_t *h, float dir); +static int foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, + FAR struct foc_angle_in_f32_s *in, + FAR struct foc_angle_out_f32_s *out); /**************************************************************************** * Public Data @@ -65,6 +68,8 @@ struct foc_angle_ops_f32_s g_foc_angle_ol_f32 = .init = foc_angle_ol_init_f32, .deinit = foc_angle_ol_deinit_f32, .cfg = foc_angle_ol_cfg_f32, + .zero = foc_angle_ol_zero_f32, + .dir = foc_angle_ol_dir_f32, .run = foc_angle_ol_run_f32, }; @@ -144,7 +149,7 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg) DEBUGASSERT(h); - /* Get modulation data */ + /* Get open-loop data */ DEBUGASSERT(h->data); ol = h->data; @@ -158,6 +163,72 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg) DEBUGASSERT(ol->cfg.per > 0.0f); motor_openloop_init(&ol->data, ol->cfg.per); + /* Initialize with CW direction */ + + ol->dir = DIR_CW; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_ol_zero_f32 + * + * Description: + * Zero the open-loop FOC angle handler (float32) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_ol_zero_f32(FAR foc_angle_f32_t *h) +{ + FAR struct foc_openloop_f32_s *ol = NULL; + + DEBUGASSERT(h); + + /* Get open-loop data */ + + DEBUGASSERT(h->data); + ol = h->data; + + /* Reset angle */ + + ol->data.angle = 0.0f; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_ol_dir_f32 + * + * Description: + * Set the open-loop FOC angle handler direction (float32) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_ol_dir_f32(FAR foc_angle_f32_t *h, float dir) +{ + FAR struct foc_openloop_f32_s *ol = NULL; + + DEBUGASSERT(h); + + UNUSED(dir); + + /* Get open-loop data */ + + DEBUGASSERT(h->data); + ol = h->data; + + /* Configure direction */ + + ol->dir = dir; + return OK; } @@ -174,7 +245,7 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg) * ****************************************************************************/ -static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, +static int foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, FAR struct foc_angle_in_f32_s *in, FAR struct foc_angle_out_f32_s *out) { @@ -182,7 +253,7 @@ static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, DEBUGASSERT(h); - /* Get modulation data */ + /* Get open-loop data */ DEBUGASSERT(h->data); ol = h->data; @@ -193,5 +264,8 @@ static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h, /* Get open-loop angle */ - out->angle = motor_openloop_angle_get(&ol->data); + out->type = FOC_ANGLE_TYPE_ELE; + out->angle = ol->dir * motor_openloop_angle_get(&ol->data); + + return OK; }