You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2023/11/18 16:39:47 UTC
(nuttx-apps) branch master updated: industry/foc: fix direction alignment
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
The following commit(s) were added to refs/heads/master by this push:
new d58944398 industry/foc: fix direction alignment
d58944398 is described below
commit d589443980d9253dec4556832701ac6bc51977c7
Author: raiden00pl <ra...@railab.me>
AuthorDate: Fri Nov 17 15:25:14 2023 +0100
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 e60be98ef..ab8d9b74e 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 16c212934..ffd6a7bb5 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
{