sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

industry/foc: fix direction alignment

Browse Source 
CCW direction wasn't properly detected when we start alignment from near zero position
This commit is contained in:
raiden00pl 2023-11-17 15:25:14 +01:00 committed by Xiang Xiao
parent 1cf9640bb6
commit d589443980
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
industry/foc/fixed16/foc_align.c
View File

@ -402,6 +402,8 @@ static void foc_align_dir_move_b16(FAR struct foc_align_b16_s *align,
static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align, static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
b16_t dir, bool last, bool diff) b16_t dir, bool last, bool diff)
{ {
b16_t tmp = 0;
DEBUGASSERT(align); DEBUGASSERT(align);
/* Lock angle */ /* Lock angle */
@ -422,13 +424,16 @@ static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
if (diff == true) if (diff == true)
{ {
tmp = align->angle_now - align->angle_last;
angle_norm_2pi_b16(&tmp, -b16PI, b16PI);
if (dir == DIR_CW_B16) if (dir == DIR_CW_B16)
{ {
align->diff_cw += (align->angle_now - align->angle_last); align->diff_cw += tmp;
} }
else if (dir == DIR_CCW_B16) else if (dir == DIR_CCW_B16)
{ {
align->diff_ccw += (align->angle_now - align->angle_last); align->diff_ccw += tmp;
} }
else else
{ {

9
industry/foc/float/foc_align.c
View File

@ -402,6 +402,8 @@ static void foc_align_dir_move_f32(FAR struct foc_align_f32_s *align,
static void foc_align_dir_hold_f32(FAR struct foc_align_f32_s *align, static void foc_align_dir_hold_f32(FAR struct foc_align_f32_s *align,
float dir, bool last, bool diff) float dir, bool last, bool diff)
{ {
float tmp = 0.0f;
DEBUGASSERT(align); DEBUGASSERT(align);
/* Lock angle */ /* Lock angle */
@ -422,13 +424,16 @@ static void foc_align_dir_hold_f32(FAR struct foc_align_f32_s *align,
if (diff == true) if (diff == true)
{ {
tmp = align->angle_now - align->angle_last;
angle_norm_2pi(&tmp, -M_PI_F, M_PI_F);
if (dir == DIR_CW) if (dir == DIR_CW)
{ {
align->diff_cw += (align->angle_now - align->angle_last); align->diff_cw += tmp;
} }
else if (dir == DIR_CCW) else if (dir == DIR_CCW)
{ {
align->diff_ccw += (align->angle_now - align->angle_last); align->diff_ccw += tmp;
} }
else else
{ {