You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by ac...@apache.org on 2022/02/14 13:47:21 UTC
[incubator-nuttx] branch master updated (8f6be5e -> 2391e4b)
This is an automated email from the ASF dual-hosted git repository.
acassis pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git.
from 8f6be5e Fix configure.sh for custom apps dir.
new 132f27a lib_observer: add nolinear fluxlink observer
new 2391e4b Update libs/libdsp/lib_observer.c
The 2 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails. The revisions
listed as "add" were already present in the repository and have only
been added to this reference.
Summary of changes:
include/dsp.h | 26 +++++-
libs/libdsp/lib_observer.c | 193 ++++++++++++++++++++++++++++++++++++++++++++-
2 files changed, 215 insertions(+), 4 deletions(-)
[incubator-nuttx] 02/02: Update libs/libdsp/lib_observer.c
Posted by ac...@apache.org.
This is an automated email from the ASF dual-hosted git repository.
acassis pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git
commit 2391e4bd714f5802ee45e65d2c473a742378e4d3
Author: zouboan <ff...@feedforward.com.cn>
AuthorDate: Sun Feb 13 23:15:17 2022 +0800
Update libs/libdsp/lib_observer.c
Co-authored-by: Mateusz Szafoni <ra...@gmail.com>
---
libs/libdsp/lib_observer.c | 4 +---
1 file changed, 1 insertion(+), 3 deletions(-)
diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c
index dfc7e6e..3cd692c 100644
--- a/libs/libdsp/lib_observer.c
+++ b/libs/libdsp/lib_observer.c
@@ -511,9 +511,7 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
* Initialize motor nolinear fluxlink observer.
*
* Input Parameters:
- * smo - pointer to the nolinear fluxlink observer private data
- * kslide - SMO gain
- * err_max - linear region upper limit
+ * nfo - pointer to the nolinear fluxlink observer private data
*
* Returned Value:
* None
[incubator-nuttx] 01/02: lib_observer: add nolinear fluxlink observer
Posted by ac...@apache.org.
This is an automated email from the ASF dual-hosted git repository.
acassis pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git
commit 132f27a91adbbc2aa93a07cc5989c6b82018e5eb
Author: zouboan <ff...@feedforward.com.cn>
AuthorDate: Sun Feb 13 13:46:40 2022 +0800
lib_observer: add nolinear fluxlink observer
---
include/dsp.h | 26 +++++-
libs/libdsp/lib_observer.c | 195 ++++++++++++++++++++++++++++++++++++++++++++-
2 files changed, 217 insertions(+), 4 deletions(-)
diff --git a/include/dsp.h b/include/dsp.h
index 923dbce..46102d0 100644
--- a/include/dsp.h
+++ b/include/dsp.h
@@ -293,12 +293,13 @@ struct motor_sobserver_div_f32_s
};
/* Speed observer PLL method data */
-#if 0
+
struct motor_sobserver_pll_f32_s
{
- /* TODO */
+ float pll_phase;
+ float pll_kp;
+ float pll_ki;
};
-#endif
/* Motor Sliding Mode Observer private data */
@@ -320,6 +321,14 @@ struct motor_observer_smo_f32_s
ab_frame_f32_t sign; /* Bang-bang controller sign */
};
+/* Motor Nonlinear FluxLink Observer private data */
+
+struct motor_observer_nfo_f32_s
+{
+ float x1;
+ float x2;
+};
+
/* FOC initialize data */
struct foc_initdata_f32_s
@@ -367,6 +376,7 @@ struct foc_data_f32_s
struct motor_phy_params_f32_s
{
uint8_t p; /* Number of the motor pole pairs */
+ float flux_link; /* Flux linkage */
float res; /* Phase-to-neutral resistance */
float ind; /* Average phase-to-neutral inductance */
float one_by_ind; /* Inverse phase-to-neutral inductance */
@@ -535,6 +545,16 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
float angle, float dir);
+void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo);
+void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
+ FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
+ FAR struct motor_phy_params_f32_s *phy, float gain);
+
+void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
+ float pll_kp, float pll_ki);
+void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
+ float angle, float dir);
+
/* Motor openloop control */
void motor_openloop_init(FAR struct openloop_data_f32_s *op, float per);
diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c
index 8e05462..dfc7e6e 100644
--- a/libs/libdsp/lib_observer.c
+++ b/libs/libdsp/lib_observer.c
@@ -31,6 +31,15 @@
#define ANGLE_DIFF_THR (M_PI_F)
+/* nan check for floats */
+
+#define IS_NAN(x) ((x) != (x))
+#define NAN_ZERO(x) (x = IS_NAN(x) ? 0.0 : x)
+
+/* Squared */
+
+#define SQ(x) ((x) * (x))
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -258,7 +267,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
{
filter = 0.99f;
}
- else if (filter <= 0.0f)
+ else if (filter < 0.005f)
{
filter = 0.005f;
}
@@ -496,6 +505,190 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
}
/****************************************************************************
+ * Name: motor_observer_nfo_init
+ *
+ * Description:
+ * Initialize motor nolinear fluxlink observer.
+ *
+ * Input Parameters:
+ * smo - pointer to the nolinear fluxlink observer private data
+ * kslide - SMO gain
+ * err_max - linear region upper limit
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo)
+{
+ LIBDSP_DEBUGASSERT(smo != NULL);
+
+ /* Reset structure */
+
+ memset(nfo, 0, sizeof(struct motor_observer_nfo_f32_s));
+}
+
+/****************************************************************************
+ * Name: motor_observer_nfo
+ *
+ * Description:
+ * nolinear fluxlink observer.
+ * REFERENCE: http://cas.ensmp.fr/~praly/Telechargement/Journaux/
+ * 2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the common observer data
+ * i_ab - (in) inverter alpha-beta current
+ * v_ab - (in) inverter alpha-beta voltage
+ * phy - (in) pointer to the motor physical parameters
+ * gain - (in) dynamic observer gain
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
+ FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
+ FAR struct motor_phy_params_f32_s *phy, float gain)
+{
+ FAR struct motor_observer_nfo_f32_s *nfo =
+ (FAR struct motor_observer_nfo_f32_s *)o->ao;
+ float angle;
+ float err;
+ float x1_dot;
+ float x2_dot;
+
+ float l_ia = (3.0 / 2.0) * phy->ind * i_ab->a;
+ float l_ib = (3.0 / 2.0) * phy->ind * i_ab->b;
+ float r_ia = (3.0 / 2.0) * phy->res * i_ab->a;
+ float r_ib = (3.0 / 2.0) * phy->res * i_ab->b;
+
+ err = SQ(phy->flux_link) - (SQ(nfo->x1 - l_ia) + SQ(nfo->x2 - l_ib));
+
+ /* Forcing this term to stay negative helps convergence according to
+ * http://cas.ensmp.fr/Publications/Publications/Papers/
+ * ObserverPermanentMagnet.pdf and
+ * https://arxiv.org/pdf/1905.00833.pdf
+ */
+
+ if (err > 0.0)
+ {
+ err = 0.0;
+ }
+
+ x1_dot = -r_ia + v_ab->a + gain * (nfo->x1 - l_ia) * err;
+ x2_dot = -r_ib + v_ab->b + gain * (nfo->x2 - l_ib) * err;
+ nfo->x1 += x1_dot * o->per;
+ nfo->x2 += x2_dot * o->per;
+
+ NAN_ZERO(nfo->x1);
+ NAN_ZERO(nfo->x2);
+
+ /* Prevent the magnitude from getting too low
+ * as that makes the angle very unstable.
+ */
+
+ if (vector2d_mag(nfo->x1, nfo->x2) < (phy->flux_link * 0.5))
+ {
+ nfo->x1 *= 1.1;
+ nfo->x2 *= 1.1;
+ }
+
+ angle = fast_atan2(nfo->x2 - l_ib, nfo->x1 - l_ia);
+
+ /* Normalize angle to range <0, 2PI> */
+
+ angle_norm_2pi(&angle, 0.0f, 2.0f * M_PI_F);
+
+ /* Store estimated angle in observer data */
+
+ o->angle = angle;
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_pll_init
+ *
+ * Description:
+ * Initialize PLL speed observer
+ *
+ * Input Parameters:
+ * so - (in/out) pointer to the PLL speed observer data
+ * pll_kp - (in) pll proportional gain
+ * pll_ki - (in) pll integral gain
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
+ float pll_kp, float pll_ki)
+{
+ LIBDSP_DEBUGASSERT(so != NULL);
+ LIBDSP_DEBUGASSERT(pll_kp > 0);
+ LIBDSP_DEBUGASSERT(pll_ki > 0.0f);
+
+ /* Reset observer data */
+
+ memset(so, 0, sizeof(struct motor_sobserver_pll_f32_s));
+
+ /* Store kp for PLL observer */
+
+ so->pll_kp = pll_kp;
+
+ /* Store ki for PLL observer speed */
+
+ so->pll_ki = pll_ki;
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_pll
+ *
+ * Description:
+ * Estimate motor electrical speed based on motor electrical angle
+ * difference.
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the common observer data
+ * angle - (in) electrical angle normalized to <0.0, 2PI>
+ * dir - (in) electrical rotation direction. Valid values:
+ * DIR_CW (1.0f) or DIR_CCW(-1.0f)
+ *
+ ****************************************************************************/
+
+void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
+ float angle, float dir)
+{
+ FAR struct motor_sobserver_pll_f32_s *so =
+ (FAR struct motor_sobserver_pll_f32_s *)o->so;
+ float delta_theta = 0.0;
+
+ NAN_ZERO(so->pll_phase);
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi(&angle, -M_PI_F, -M_PI_F);
+
+ delta_theta = angle - so->pll_phase;
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi(&delta_theta, -M_PI_F, -M_PI_F);
+
+ NAN_ZERO(o->speed);
+
+ so->pll_phase += (o->speed + so->pll_kp * delta_theta) * o->per;
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi(&so->pll_phase, -M_PI_F, -M_PI_F);
+
+ o->speed += so->pll_ki * delta_theta * o->per;
+}
+
+/****************************************************************************
* Name: motor_observer_speed_get
*
* Description: