From f1895068d77456ffd69141a10e40a8380b869642 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Fri, 17 Nov 2023 15:25:14 +0100 Subject: [PATCH] industry/foc: fix direction alignment CCW direction wasn't properly detected when we start alignment from near zero position --- industry/foc/fixed16/foc_align.c | 9 +++++++-- industry/foc/float/foc_align.c | 9 +++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/industry/foc/fixed16/foc_align.c b/industry/foc/fixed16/foc_align.c index e60be98ef0d..ab8d9b74efb 100644 --- a/industry/foc/fixed16/foc_align.c +++ b/industry/foc/fixed16/foc_align.c @@ -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, b16_t dir, bool last, bool diff) { + b16_t tmp = 0; + DEBUGASSERT(align); /* Lock angle */ @@ -422,13 +424,16 @@ static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align, 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 += (align->angle_now - align->angle_last); + align->diff_cw += tmp; } else if (dir == DIR_CCW_B16) { - align->diff_ccw += (align->angle_now - align->angle_last); + align->diff_ccw += tmp; } else { diff --git a/industry/foc/float/foc_align.c b/industry/foc/float/foc_align.c index 16c21293405..ffd6a7bb5ed 100644 --- a/industry/foc/float/foc_align.c +++ b/industry/foc/float/foc_align.c @@ -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, float dir, bool last, bool diff) { + float tmp = 0.0f; + DEBUGASSERT(align); /* Lock angle */ @@ -422,13 +424,16 @@ static void foc_align_dir_hold_f32(FAR struct foc_align_f32_s *align, if (diff == true) { + tmp = align->angle_now - align->angle_last; + angle_norm_2pi(&tmp, -M_PI_F, M_PI_F); + if (dir == DIR_CW) { - align->diff_cw += (align->angle_now - align->angle_last); + align->diff_cw += tmp; } else if (dir == DIR_CCW) { - align->diff_ccw += (align->angle_now - align->angle_last); + align->diff_ccw += tmp; } else {