From 768d5b29f10972ea33a42ee53375d0c748114d20 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Tue, 10 Oct 2023 13:15:49 +0200
Subject: [PATCH] industry/foc: force angle observers output to zero if motor
 stopped

this prevents junk data from the observer when the motor is stopped
---
 industry/foc/fixed16/foc_ang_onfo.c | 9 +++++++++
 industry/foc/fixed16/foc_ang_osmo.c | 9 +++++++++
 industry/foc/float/foc_ang_onfo.c   | 9 +++++++++
 industry/foc/float/foc_ang_osmo.c   | 9 +++++++++
 4 files changed, 36 insertions(+)

diff --git a/industry/foc/fixed16/foc_ang_onfo.c b/industry/foc/fixed16/foc_ang_onfo.c
index d778a2586..f5c2c453f 100644
--- a/industry/foc/fixed16/foc_ang_onfo.c
+++ b/industry/foc/fixed16/foc_ang_onfo.c
@@ -281,6 +281,15 @@ static int foc_angle_onfo_run_b16(FAR foc_angle_b16_t *h,
   DEBUGASSERT(h->data);
   ob = h->data;
 
+  if (in->vel == 0)
+    {
+      /* Do nothing if motor stopped */
+
+      out->type  = FOC_ANGLE_TYPE_ELE;
+      out->angle = 0;
+      return OK;
+    }
+
   /* Normalize the d-q voltage to get the d-q modulation
    * voltage
    */
diff --git a/industry/foc/fixed16/foc_ang_osmo.c b/industry/foc/fixed16/foc_ang_osmo.c
index 90acd2a86..d4c6d3152 100644
--- a/industry/foc/fixed16/foc_ang_osmo.c
+++ b/industry/foc/fixed16/foc_ang_osmo.c
@@ -268,6 +268,15 @@ static int foc_angle_osmo_run_b16(FAR foc_angle_b16_t *h,
   DEBUGASSERT(h->data);
   ob = h->data;
 
+  if (in->vel == 0)
+    {
+      /* Do nothing if motor stopped */
+
+      out->type  = FOC_ANGLE_TYPE_ELE;
+      out->angle = 0;
+      return OK;
+    }
+
   /* Update observer */
 
   motor_aobserver_smo_b16(&ob->o, &in->state->iab, &in->state->vab,
diff --git a/industry/foc/float/foc_ang_onfo.c b/industry/foc/float/foc_ang_onfo.c
index 39e2227be..665eba832 100644
--- a/industry/foc/float/foc_ang_onfo.c
+++ b/industry/foc/float/foc_ang_onfo.c
@@ -280,6 +280,15 @@ static int foc_angle_onfo_run_f32(FAR foc_angle_f32_t *h,
   DEBUGASSERT(h->data);
   ob = h->data;
 
+  if (in->vel == 0.0f)
+    {
+      /* Do nothing if motor stopped */
+
+      out->type  = FOC_ANGLE_TYPE_ELE;
+      out->angle = 0.0f;
+      return OK;
+    }
+
   /* Normalize the d-q voltage to get the d-q modulation voltage */
 
   v_dq_mod.d = in->state->vdq.d * in->state->mod_scale;
diff --git a/industry/foc/float/foc_ang_osmo.c b/industry/foc/float/foc_ang_osmo.c
index ce693f3b7..4d31a399c 100644
--- a/industry/foc/float/foc_ang_osmo.c
+++ b/industry/foc/float/foc_ang_osmo.c
@@ -268,6 +268,15 @@ static int foc_angle_osmo_run_f32(FAR foc_angle_f32_t *h,
   DEBUGASSERT(h->data);
   ob = h->data;
 
+  if (in->vel == 0.0f)
+    {
+      /* Do nothing if motor stopped */
+
+      out->type  = FOC_ANGLE_TYPE_ELE;
+      out->angle = 0.0f;
+      return OK;
+    }
+
   /* Update observer */
 
   motor_aobserver_smo(&ob->o, &in->state->iab, &in->state->vab,