industry/foc/*/foc_openloop.c: adapt to previous changes
This commit is contained in:
raiden00pl
Xiang Xiao
parent
515a25fbb8
commit
ff839dcf3e
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user