sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
25e8c1c9c4
nuttx-apps/industry/foc/fixed16/foc_align.c
raiden00pl d589443980 industry/foc: fix direction alignment 
CCW direction wasn't properly detected when we start alignment from near zero position
2023-11-18 08:39:40 -08:00

1022 lines
24 KiB
C
Raw Blame History

/****************************************************************************
* apps/industry/foc/fixed16/foc_align.c
* This file implements angle sensor alignment routine for fixed16
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/foc_common.h"
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_align.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Align to -90deg = 270deg */
#define FOC_ALIGN_ANGLE (b16muli(b16HALFPI, 3))
/* Direction alignment */
#define ALIGN_DIR_ANGLE_STEP (ftob16(0.001f))
#define ALIGN_DIR_ANGLE_HOLD_0 (ftob16(0.0f))
#define ALIGN_DIR_ANGLE_HOLD_1 (b16divi(b16PI, 3))
#define ALIGN_DIR_ANGLE_HOLD_2 (b16muli(b16divi(b16PI, 3), 2))
#define ALIGN_DIR_ANGLE_HOLD_3 (b16PI)
#define ALIGN_DIR_ANGLE_HOLD_4 (b16muli(b16divi(b16PI, 3), 4))
#define ALIGN_DIR_HOLD_CNTR (10)
/* IDLE steps */
#define IDLE_STEPS (500)
/* Index alignment */
#define ALIGN_INDEX_ANGLE_SIGN (-b16muli(b16PI, 2))
#define ALIGN_INDEX_ANGLE_STEP (ftob16(0.001f))
#define ALIGN_INDEX_ANGLE_NOZERO (ftob16(0.1f))
/****************************************************************************
* Private Data Types
****************************************************************************/
/* Align stage */
enum foc_align_run_stage_e
{
FOC_ALIGN_RUN_INIT,
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
FOC_ALIGN_RUN_INDEX,
#endif
FOC_ALIGN_RUN_OFFSET,
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_DIR
FOC_ALIGN_RUN_DIR,
#endif
FOC_ALIGN_RUN_IDLE,
FOC_ALIGN_RUN_DONE
};
/* Align routine private data */
struct foc_align_b16_s
{
struct foc_routine_align_cfg_b16_s cfg;
struct foc_routine_aling_final_b16_s final;
int cntr;
int stage;
int dir_step;
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
int index_step;
b16_t index_angle;
#endif
b16_t dir_angle;
b16_t angle_last;
b16_t angle_now;
b16_t diff_cw;
b16_t diff_ccw;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
int foc_routine_align_init_b16(FAR foc_routine_b16_t *r);
void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r);
int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out);
int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
/****************************************************************************
* Public Data
****************************************************************************/
struct foc_routine_ops_b16_s g_foc_routine_align_b16 =
{
.init = foc_routine_align_init_b16,
.deinit = foc_routine_align_deinit_b16,
.cfg = foc_routine_align_cfg_b16,
.run = foc_routine_align_run_b16,
.final = foc_routine_align_final_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
/****************************************************************************
* Name: foc_align_index_run_b16
*
* Description:
* Run index alignment routine
*
* Input Parameter:
* align - pointer to FOC align routine
* in - pointer to FOC routine input data
* out - pointer to FOC routine output data
*
****************************************************************************/
static int foc_align_index_run_b16(FAR struct foc_align_b16_s *align,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out)
{
static b16_t dir = DIR_CW_B16;
b16_t sign = 0;
int ret = FOC_ROUTINE_RUN_NOTDONE;
/* Get mechanical angles */
align->angle_last = align->angle_now;
align->angle_now = in->angle_m;
/* Normalize angle to <-M_PI, M_PI> range */
align->angle_now = align->angle_now - b16PI;
angle_norm_2pi_b16(&align->angle_now, -b16PI, b16PI);
/* The product of the previous angle with an angle now gives us
* information about the encoder overflow.
* This procedure assumes that the encoder index signal resets the
* encoder value to zero.
*/
sign = b16mulb16(align->angle_now, align->angle_last);
/* Move the motor shaft to approach index signal from both direction.
* After that, the encoder zero should be aligned with the encoder index.
*/
if (align->index_step == 0)
{
/* Move the motor shaft away from zero */
align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
{
align->index_step += 1;
}
}
else if (align->index_step == 1)
{
/* Move the motor shaft to zero in positive direction */
align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
{
dir = b16mulb16(DIR_CCW_B16, dir);
align->index_step += 1;
}
}
else if (align->index_step == 2)
{
/* Move the motor shaft away from zero */
align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
{
align->index_step += 1;
}
}
else if (align->index_step == 3)
{
/* Move the motor shaft to zero in negative direction */
align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
{
dir = b16mulb16(DIR_CCW_B16, dir);
align->index_step += 1;
}
}
else if (align->index_step == 4)
{
/* Done */
ret = FOC_ROUTINE_RUN_DONE;
}
/* Get output */
if (ret == FOC_ROUTINE_RUN_DONE)
{
/* Force IDLE mode */
out->dq_ref.q = 0;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = 0;
out->foc_mode = FOC_HANDLER_MODE_IDLE;
/* Reset data */
align->angle_last = 0;
align->angle_now = 0;
align->cntr = 0;
}
else
{
/* Force DQ voltage vector */
out->dq_ref.q = align->cfg.volt;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = align->index_angle;
out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
/* Increase counter */
align->cntr += 1;
}
return ret;
}
#endif
/****************************************************************************
* Name: foc_align_offset_run_b16
*
* Description:
* Run offset alignment routine
*
* Input Parameter:
* align - pointer to FOC align routine
* in - pointer to FOC routine input data
* out - pointer to FOC routine output data
*
****************************************************************************/
static int foc_align_offset_run_b16(FAR struct foc_align_b16_s *align,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out)
{
int ret = FOC_ROUTINE_RUN_NOTDONE;
/* Reset Align angle sensor */
if (align->cntr > align->cfg.offset_steps)
{
/* Align angle sensor */
if (align->cfg.cb.zero != NULL)
{
ret = align->cfg.cb.zero(align->cfg.cb.priv, in->angle);
if (ret < 0)
{
FOCLIBERR("ERROR: align offset callback failed %d!\n", ret);
goto errout;
}
}
/* Offset align done */
align->cntr = 0;
ret = FOC_ROUTINE_RUN_DONE;
/* Force IDLE mode */
out->dq_ref.q = 0;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = 0;
out->foc_mode = FOC_HANDLER_MODE_IDLE;
}
else
{
/* Force DQ voltage vector */
out->dq_ref.q = align->cfg.volt;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = FOC_ALIGN_ANGLE;
out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
/* Increase counter */
align->cntr += 1;
}
/* Store offset if done */
if (ret == FOC_ROUTINE_RUN_DONE)
{
align->final.offset = in->angle;
}
errout:
return ret;
}
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_DIR
/****************************************************************************
* Name: foc_align_dir_move_b16
****************************************************************************/
static void foc_align_dir_move_b16(FAR struct foc_align_b16_s *align,
b16_t dir, b16_t dest)
{
bool next = false;
DEBUGASSERT(align);
/* Update angle */
align->dir_angle += b16mulb16(dir, ALIGN_DIR_ANGLE_STEP);
/* Check destination */
if (dir == DIR_CW_B16)
{
if (align->dir_angle >= dest)
{
next = true;
}
}
else if (dir == DIR_CCW_B16)
{
if (align->dir_angle <= dest)
{
next = true;
}
}
else
{
DEBUGASSERT(0);
}
/* Next step */
if (next == true)
{
align->dir_step += 1;
}
}
/****************************************************************************
* Name: foc_align_dir_hold_b16
****************************************************************************/
static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
b16_t dir, bool last, bool diff)
{
b16_t tmp = 0;
DEBUGASSERT(align);
/* Lock angle */
align->dir_angle = align->dir_angle;
/* Increase counter */
align->cntr += 1;
if (align->cntr > ALIGN_DIR_HOLD_CNTR)
{
/* Next step */
align->dir_step += 1;
/* Accumulate diff */
if (diff == true)
{
tmp = align->angle_now - align->angle_last;
angle_norm_2pi_b16(&tmp, -b16PI, b16PI);
if (dir == DIR_CW_B16)
{
align->diff_cw += tmp;
}
else if (dir == DIR_CCW_B16)
{
align->diff_ccw += tmp;
}
else
{
DEBUGASSERT(0);
}
}
/* Store last angle */
if (last == true)
{
align->angle_last = align->angle_now;
}
/* Reset counter */
align->cntr = 0;
}
}
/****************************************************************************
* Name: foc_align_dir_run_b16
*
* Description:
* Run direction alignment routine
*
* Input Parameter:
* align - pointer to FOC align routine
* in - pointer to FOC routine input data
* out - pointer to FOC routine output data
*
****************************************************************************/
int foc_align_dir_run_b16(FAR struct foc_align_b16_s *align,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out)
{
int ret = FOC_ROUTINE_RUN_NOTDONE;
bool last = false;
/* Store angle now */
align->angle_now = in->angle;
/* Handle dir align */
if (align->dir_step == 0)
{
/* Start with position 0 */
align->dir_angle = ALIGN_DIR_ANGLE_HOLD_0;
/* Step 0 - hold position 0 */
foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
}
else if (align->dir_step == 1)
{
/* Step 1 - move motor in CW direction to position 1 */
foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_1);
}
else if (align->dir_step == 2)
{
/* Step 2 - hold position 1 */
foc_align_dir_hold_b16(align, DIR_CW_B16, true, false);
}
else if (align->dir_step == 3)
{
/* Step 3 - move motor in CW direction to position 2 */
foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_2);
}
else if (align->dir_step == 4)
{
/* Step 4 - hold position 2 */
foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
}
else if (align->dir_step == 5)
{
/* Step 5 - move motor in CW direction to position 3 */
foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_3);
}
else if (align->dir_step == 6)
{
/* Step 6 - hold position 3 */
foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
}
else if (align->dir_step == 6)
{
/* Step 6 - move motor in CW direction to position 4 */
foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_4);
}
else if (align->dir_step == 7)
{
/* Step 7 - hold position 4 */
foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
}
else if (align->dir_step == 8)
{
/* Step 8 - move motor in CCW direction to position 3 */
foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_3);
}
else if (align->dir_step == 9)
{
/* Step 9 - hold position 3 */
foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
}
else if (align->dir_step == 10)
{
/* Step 10 - move motor in CCW direction to position 2 */
foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_2);
}
else if (align->dir_step == 11)
{
/* Step 11 - hold position 2 */
foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
}
else if (align->dir_step == 12)
{
/* Step 12 - move motor in CCW direction to position 1 */
foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_1);
}
else if (align->dir_step == 13)
{
/* Step 13 - hold position 1 */
foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
}
else if (align->dir_step == 14)
{
/* Step 14 - move motor in CCW direction to position 0 */
foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_0);
}
else if (align->dir_step == 15)
{
/* Step 15 - hold position 0 */
foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
}
else
{
/* Set flag */
last = true;
/* Get sensor direction according to sampled data */
if (align->diff_cw > 0 && align->diff_ccw < 0)
{
align->final.dir = DIR_CW_B16;
}
else if (align->diff_cw < 0 && align->diff_ccw > 0)
{
align->final.dir = DIR_CCW_B16;
}
else
{
/* Invalid data */
FOCLIBERR("ERROR: direction align failed !\n");
align->final.dir = DIR_NONE_B16;
ret = -EINVAL;
goto errout;
}
/* Align angle sensor */
if (align->cfg.cb.dir != NULL)
{
ret = align->cfg.cb.dir(align->cfg.cb.priv, align->final.dir);
if (ret < 0)
{
FOCLIBERR("ERROR: align dir callback failed %d!\n", ret);
goto errout;
}
}
/* Direction alignment done */
ret = FOC_ROUTINE_RUN_DONE;
}
/* Force DQ voltage vector */
out->dq_ref.q = align->cfg.volt;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = align->dir_angle;
out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
errout:
/* Force current to zero if alignment done */
if (last == true)
{
out->dq_ref.q = 0;
out->dq_ref.d = 0;
}
return ret;
}
#endif
/****************************************************************************
* Name: foc_align_idle_run_b16
*
* Description:
* Force IDLE state
*
* Input Parameter:
* align - pointer to FOC align routine
* in - pointer to FOC routine input data
* out - pointer to FOC routine output data
*
****************************************************************************/
static int foc_align_idle_run_b16(FAR struct foc_align_b16_s *align,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out)
{
int ret = FOC_ROUTINE_RUN_NOTDONE;
/* Get output */
out->dq_ref.q = 0;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = 0;
out->foc_mode = FOC_HANDLER_MODE_IDLE;
/* Increase counter */
align->cntr += 1;
if (align->cntr > IDLE_STEPS)
{
/* Done */
ret = FOC_ROUTINE_RUN_DONE;
/* Reset counter */
align->cntr = 0;
}
return ret;
}
/****************************************************************************
* Name: foc_routine_align_init_b16
*
* Description:
* Initialize the FOC align routine (fixed16)
*
* Input Parameter:
* r - pointer to FOC routine
*
****************************************************************************/
int foc_routine_align_init_b16(FAR foc_routine_b16_t *r)
{
int ret = OK;
DEBUGASSERT(r);
/* Connect angle data */
r->data = zalloc(sizeof(struct foc_align_b16_s));
if (r->data == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_routine_align_deinit_b16
*
* Description:
* Deinitialize the FOC align routine (fixed16)
*
* Input Parameter:
* r - pointer to FOC routine
*
****************************************************************************/
void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r)
{
DEBUGASSERT(r);
if (r->data)
{
/* Free routine data */
free(r->data);
}
}
/****************************************************************************
* Name: foc_routine_align_cfg_b16
*
* Description:
* Configure the FOC align routine (fixed16)
*
* Input Parameter:
* r - pointer to FOC routine
* cfg - pointer to align routine configuration data
*
****************************************************************************/
int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
{
FAR struct foc_align_b16_s *a = NULL;
int ret = OK;
DEBUGASSERT(r);
DEBUGASSERT(cfg);
/* Get aling data */
DEBUGASSERT(r->data);
a = r->data;
/* Copy configuration */
memcpy(&a->cfg, cfg, sizeof(struct foc_routine_align_cfg_b16_s));
/* Verify configuration */
if (a->cfg.volt <= 0)
{
ret = -EINVAL;
goto errout;
}
if (a->cfg.offset_steps <= 0)
{
ret = -EINVAL;
goto errout;
}
if (a->cfg.cb.zero == NULL || a->cfg.cb.dir == NULL ||
a->cfg.cb.priv == NULL)
{
FOCLIBWARN("WARN: no align cb data!\n");
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_routine_align_run_b16
*
* Description:
* Run the FOC align routine step (fixed16)
*
* Input Parameter:
* r - pointer to FOC routine
* in - pointer to FOC routine input data
* out - pointer to FOC routine output data
*
****************************************************************************/
int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
FAR struct foc_routine_in_b16_s *in,
FAR struct foc_routine_out_b16_s *out)
{
FAR struct foc_align_b16_s *a = NULL;
int ret = FOC_ROUTINE_RUN_NOTDONE;
DEBUGASSERT(r);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Get aling data */
DEBUGASSERT(r->data);
a = r->data;
/* Force IDLE state at default */
out->dq_ref.q = 0;
out->dq_ref.d = 0;
out->vdq_comp.q = 0;
out->vdq_comp.d = 0;
out->angle = 0;
out->foc_mode = FOC_HANDLER_MODE_IDLE;
/* Handle alignment stage */
switch (a->stage)
{
case FOC_ALIGN_RUN_INIT:
{
a->stage += 1;
ret = FOC_ROUTINE_RUN_NOTDONE;
break;
}
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
case FOC_ALIGN_RUN_INDEX:
{
/* Align zero procedure */
ret = foc_align_index_run_b16(a, in, out);
if (ret < 0)
{
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
FOCLIBLOG("ALIGN INDEX done!\n");
a->stage += 1;
ret = FOC_ROUTINE_RUN_NOTDONE;
}
break;
}
#endif
case FOC_ALIGN_RUN_OFFSET:
{
/* Align zero procedure */
ret = foc_align_offset_run_b16(a, in, out);
if (ret < 0)
{
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
FOCLIBLOG("ALIGN OFFSET done!\n");
a->stage += 1;
ret = FOC_ROUTINE_RUN_NOTDONE;
}
break;
}
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_DIR
case FOC_ALIGN_RUN_DIR:
{
/* Align direction procedure */
ret = foc_align_dir_run_b16(a, in, out);
if (ret < 0)
{
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
FOCLIBLOG("ALIGN DIR done!\n");
a->stage += 1;
ret = FOC_ROUTINE_RUN_NOTDONE;
}
break;
}
#endif
case FOC_ALIGN_RUN_IDLE:
{
/* De-energetize motor */
ret = foc_align_idle_run_b16(a, in, out);
if (ret < 0)
{
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
FOCLIBLOG("ALIGN IDLE done!\n");
a->stage += 1;
ret = FOC_ROUTINE_RUN_NOTDONE;
}
break;
}
case FOC_ALIGN_RUN_DONE:
{
ret = FOC_ROUTINE_RUN_DONE;
break;
}
default:
{
FOCLIBERR("ERROR: invalid alignment stage %d\n", a->stage);
ret = -EINVAL;
goto errout;
}
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_routine_align_final_b16
*
* Description:
* Finalize the FOC routine data (fixed16)
*
* Input Parameter:
* r - pointer to FOC routine
* data - pointer to FOC align routine final data
*
****************************************************************************/
int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
{
FAR struct foc_align_b16_s *a = NULL;
DEBUGASSERT(r);
DEBUGASSERT(data);
/* Get aling data */
DEBUGASSERT(r->data);
a = r->data;
/* Get final data */
memcpy(data, &a->final, sizeof(struct foc_routine_aling_final_b16_s));
/* Reset data but leave configuration */
memset(&a->final,
0,
(sizeof(struct foc_align_b16_s) -
sizeof(struct foc_routine_align_cfg_b16_s)));
return OK;
}
Reference in New Issue View Git Blame Copy Permalink