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 2022/02/20 13:58:23 UTC
[incubator-nuttx] 04/04: libdsp: port lib_observer.c to b16
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/incubator-nuttx.git
commit 4e27f4a1d20211aafd965136a3e0ce44ca5c8e4a
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Feb 19 21:12:20 2022 +0100
libdsp: port lib_observer.c to b16
---
include/dspb16.h | 112 ++++++
libs/libdsp/Makefile | 1 +
libs/libdsp/lib_observer_b16.c | 757 +++++++++++++++++++++++++++++++++++++++++
3 files changed, 870 insertions(+)
diff --git a/include/dspb16.h b/include/dspb16.h
index 5763627..ee9f23e 100644
--- a/include/dspb16.h
+++ b/include/dspb16.h
@@ -247,6 +247,84 @@ struct openloop_data_b16_s
b16_t per; /* Open-loop control execution period */
};
+/* Common motor speed observer structure */
+
+struct motor_sobserver_b16_s
+{
+ b16_t speed; /* Estimated observer speed */
+ b16_t per; /* Observer execution period */
+
+ /* There are different types of motor observers which different
+ * sets of private data.
+ */
+
+ void *so; /* Speed estimation observer data */
+};
+
+/* Common motor angle observer structure */
+
+struct motor_aobserver_b16_s
+{
+ b16_t angle; /* Estimated observer angle */
+ b16_t per; /* Observer execution period */
+
+ /* There are different types of motor observers which different
+ * sets of private data.
+ */
+
+ void *ao; /* Angle estimation observer data */
+};
+
+/* Speed observer division method data */
+
+struct motor_sobserver_div_b16_s
+{
+ b16_t angle_diff; /* Angle difference */
+ b16_t angle_acc; /* Accumulated angle */
+ b16_t angle_prev; /* Previous angle */
+ b16_t one_by_dt; /* Frequency of observer execution */
+ b16_t cntr; /* Sample counter */
+ b16_t samples; /* Number of samples for observer */
+ b16_t filter; /* Low-pass filter for final omega */
+};
+
+/* Speed observer PLL method data */
+
+struct motor_sobserver_pll_b16_s
+{
+ b16_t pll_phase;
+ b16_t pll_kp;
+ b16_t pll_ki;
+};
+
+/* Motor Sliding Mode Observer private data */
+
+struct motor_aobserver_smo_b16_s
+{
+ b16_t k_slide; /* Bang-bang controller gain */
+ b16_t err_max; /* Linear mode threshold */
+ b16_t one_by_err_max; /* One by err_max */
+ b16_t F; /* Current observer F gain (1-Ts*R/L) */
+ b16_t G; /* Current observer G gain (Ts/L) */
+ b16_t emf_lp_filter1; /* Adaptive first low pass EMF filter */
+ b16_t emf_lp_filter2; /* Adaptive second low pass EMF filter */
+ ab_frame_b16_t emf; /* Estimated back-EMF */
+ ab_frame_b16_t emf_f; /* Fitlered estimated back-EMF */
+ ab_frame_b16_t z; /* Correction factor */
+ ab_frame_b16_t i_est; /* Estimated idq current */
+ ab_frame_b16_t v_err; /* v_err = v_ab - emf */
+ ab_frame_b16_t i_err; /* i_err = i_est - i_dq */
+ ab_frame_b16_t sign; /* Bang-bang controller sign */
+};
+
+/* Motor Nonlinear FluxLink Observer private data */
+
+struct motor_aobserver_nfo_b16_s
+{
+ b16_t x1;
+ b16_t x2;
+};
+
/* FOC initialize data */
struct foc_initdata_b16_s
@@ -445,6 +523,40 @@ void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc,
FAR ab_frame_b16_t *v_ab_mod);
void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max);
+/* BLDC/PMSM motor observers */
+
+void motor_sobserver_init_b16(FAR struct motor_sobserver_b16_s *observer,
+ FAR void *so, b16_t per);
+void motor_aobserver_init_b16(FAR struct motor_aobserver_b16_s *observer,
+ FAR void *ao, b16_t per);
+b16_t motor_sobserver_speed_get_b16(FAR struct motor_sobserver_b16_s *o);
+b16_t motor_aobserver_angle_get_b16(FAR struct motor_aobserver_b16_s *o);
+
+void motor_aobserver_smo_init_b16(FAR struct motor_aobserver_smo_b16_s *smo,
+ b16_t kslide, b16_t err_max);
+void motor_aobserver_smo_b16(FAR struct motor_aobserver_b16_s *o,
+ FAR ab_frame_b16_t *i_ab,
+ FAR ab_frame_b16_t *v_ab,
+ FAR struct motor_phy_params_b16_s *phy,
+ b16_t dir, b16_t speed);
+
+void motor_sobserver_div_init_b16(FAR struct motor_sobserver_div_b16_s *so,
+ uint8_t samples, b16_t filer, b16_t per);
+void motor_sobserver_div_b16(FAR struct motor_sobserver_b16_s *o,
+ b16_t angle);
+
+void motor_aobserver_nfo_init_b16(FAR struct motor_aobserver_nfo_b16_s *nfo);
+void motor_aobserver_nfo_b16(FAR struct motor_aobserver_b16_s *o,
+ FAR ab_frame_b16_t *i_ab,
+ FAR ab_frame_b16_t *v_ab,
+ FAR struct motor_phy_params_b16_s *phy,
+ b16_t gain);
+
+void motor_sobserver_pll_init_b16(FAR struct motor_sobserver_pll_b16_s *so,
+ b16_t pll_kp, b16_t pll_ki);
+void motor_sobserver_pll_b16(FAR struct motor_sobserver_b16_s *o,
+ b16_t angle);
+
/* Motor openloop control */
void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per);
diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile
index b308cfc..cb2c72e 100644
--- a/libs/libdsp/Makefile
+++ b/libs/libdsp/Makefile
@@ -33,6 +33,7 @@ CSRCS += lib_pmsm_model.c
CSRCS += lib_pid_b16.c
CSRCS += lib_svm_b16.c
CSRCS += lib_transform_b16.c
+CSRCS += lib_observer_b16.c
CSRCS += lib_foc_b16.c
CSRCS += lib_misc_b16.c
CSRCS += lib_motor_b16.c
diff --git a/libs/libdsp/lib_observer_b16.c b/libs/libdsp/lib_observer_b16.c
new file mode 100644
index 0000000..10474dc
--- /dev/null
+++ b/libs/libdsp/lib_observer_b16.c
@@ -0,0 +1,757 @@
+/****************************************************************************
+ * libs/libdsp/lib_observer_b16.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <dspb16.h>
+#include <string.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Squared */
+
+#define SQ_B16(x) (b16mulb16((x), (x)))
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: motor_sobserver_init_b16
+ *
+ * Description:
+ * Initialize motor speed observer
+ *
+ * Input Parameters:
+ * observer - pointer to the speed observer data
+ * so - pointer to the speed specific observer data
+ * per - observer execution period
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_sobserver_init_b16(FAR struct motor_sobserver_b16_s *observer,
+ FAR void *so, b16_t per)
+{
+ LIBDSP_DEBUGASSERT(observer != NULL);
+ LIBDSP_DEBUGASSERT(so != NULL);
+ LIBDSP_DEBUGASSERT(per > 0);
+
+ /* Reset observer data */
+
+ memset(observer, 0, sizeof(struct motor_sobserver_b16_s));
+
+ /* Set observer period */
+
+ observer->per = per;
+
+ /* Connect speed estimation observer data */
+
+ observer->so = so;
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_init_b16
+ *
+ * Description:
+ * Initialize motor angle observer
+ *
+ * Input Parameters:
+ * observer - pointer to the angle observer data
+ * ao - pointer to the angle specific observer data
+ * per - observer execution period
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_aobserver_init_b16(FAR struct motor_aobserver_b16_s *observer,
+ FAR void *ao, b16_t per)
+{
+ LIBDSP_DEBUGASSERT(observer != NULL);
+ LIBDSP_DEBUGASSERT(ao != NULL);
+ LIBDSP_DEBUGASSERT(per > 0);
+
+ /* Reset observer data */
+
+ memset(observer, 0, sizeof(struct motor_aobserver_b16_s));
+
+ /* Set observer period */
+
+ observer->per = per;
+
+ /* Connect angle estimation observer data */
+
+ observer->ao = ao;
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_smo_init_b16
+ *
+ * Description:
+ * Initialize motor sliding mode observer.
+ *
+ * Input Parameters:
+ * smo - pointer to the sliding mode observer private data
+ * kslide - SMO gain
+ * err_max - linear region upper limit
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_aobserver_smo_init_b16(FAR struct motor_aobserver_smo_b16_s *smo,
+ b16_t kslide, b16_t err_max)
+{
+ LIBDSP_DEBUGASSERT(smo != NULL);
+ LIBDSP_DEBUGASSERT(kslide > 0);
+ LIBDSP_DEBUGASSERT(err_max > 0);
+
+ /* Reset structure */
+
+ memset(smo, 0, sizeof(struct motor_aobserver_smo_b16_s));
+
+ /* Initialize structure */
+
+ smo->k_slide = kslide;
+ smo->err_max = err_max;
+
+ /* Store inverted err_max to avoid division */
+
+ smo->one_by_err_max = b16divb16(b16ONE, err_max);
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_smo_b16
+ *
+ * Description:
+ * One step of the SMO observer.
+ * REFERENCE: http://ww1.microchip.com/downloads/en/AppNotes/01078B.pdf
+ *
+ * Below some theoretical backgrounds about SMO.
+ *
+ * The digitalized motor model can be represent as:
+ *
+ * d(i_s.)/dt = (-R/L)*i_s. + (1/L)*(v_s - e_s. - z)
+ *
+ * We compare estimated current (i_s.) with measured current (i_s):
+ *
+ * err = i_s. - i_s
+ *
+ * and get correction factor (z):
+ *
+ * sign = sing(err)
+ * z = sign*K_SLIDE
+ *
+ * Once the digitalized model is compensated, we estimate BEMF (e_s.) by
+ * filtering z:
+ *
+ * e_s. = low_pass(z)
+ *
+ * The estimated BEMF is filtered once again and used to approximate the
+ * motor angle:
+ *
+ * e_filtered_s. = low_pass(e_s.)
+ * theta = arctan(-e_alpha/e_beta)
+ *
+ * The estimated theta is phase-shifted due to low pass filtration, so we
+ * need some phase compensation. More details below.
+ *
+ * where:
+ * v_s - phase input voltage vector
+ * i_s. - estimated phase current vector
+ * i_s - phase current vector
+ * e_s. - estimated phase BEMF vector
+ * R - motor winding resistance
+ * L - motor winding inductance
+ * z - output correction factor voltage
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the angle observer data
+ * i_ab - (in) inverter alpha-beta current
+ * v_ab - (in) inverter alpha-beta voltage
+ * phy - (in) pointer to the motor physical parameters
+ * dir - (in) rotation direction (1.0 for CCW, -1.0 for CW)
+ * NOTE: (mechanical dir) = -(electrical dir)
+ * speed - (in) electrical speed
+ * TODO: pass rotation direction with speed sign
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_aobserver_smo_b16(FAR struct motor_aobserver_b16_s *o,
+ FAR ab_frame_b16_t *i_ab, FAR ab_frame_b16_t *v_ab,
+ FAR struct motor_phy_params_b16_s *phy, b16_t dir,
+ b16_t speed)
+{
+ LIBDSP_DEBUGASSERT(o != NULL);
+ LIBDSP_DEBUGASSERT(i_ab != NULL);
+ LIBDSP_DEBUGASSERT(v_ab != NULL);
+ LIBDSP_DEBUGASSERT(phy != NULL);
+
+ FAR struct motor_aobserver_smo_b16_s *smo =
+ (FAR struct motor_aobserver_smo_b16_s *)o->ao;
+ FAR ab_frame_b16_t *emf = &smo->emf;
+ FAR ab_frame_b16_t *emf_f = &smo->emf_f;
+ FAR ab_frame_b16_t *z = &smo->z;
+ FAR ab_frame_b16_t *i_est = &smo->i_est;
+ FAR ab_frame_b16_t *v_err = &smo->v_err;
+ FAR ab_frame_b16_t *i_err = &smo->i_err;
+ FAR ab_frame_b16_t *sign = &smo->sign;
+ b16_t i_err_a_abs = 0;
+ b16_t i_err_b_abs = 0;
+ b16_t angle = 0;
+ b16_t filter = 0;
+
+ LIBDSP_DEBUGASSERT(smo != NULL);
+
+ /* REVISIT: observer works only when IQ current is high enough
+ * Lower IQ current -> lower K_SLIDE
+ */
+
+ /* Calculate observer gains */
+
+ smo->F = (b16ONE - b16mulb16(b16mulb16(o->per, phy->res),
+ phy->one_by_ind));
+ smo->G = b16mulb16(o->per, phy->one_by_ind);
+
+ /* Saturate F gain */
+
+ if (smo->F < 0)
+ {
+ smo->F = 0;
+ }
+
+ /* Saturate G gain */
+
+ if (smo->G > ftob16(0.999f))
+ {
+ smo->G = ftob16(0.999f);
+ }
+
+ /* Configure low pass filters
+ *
+ * We tune low-pass filters to achieve cutoff frequency equal to
+ * input singal frequency. This gives us constant phase shift between
+ * input and outpu signals equals to:
+ *
+ * phi = -arctan(f_in/f_c) = -arctan(1) = -45deg = -PI/4
+ *
+ * Input signal frequency is equal to the frequency of the motor currents,
+ * which give us:
+ *
+ * f_c = omega_e/(2*PI)
+ * omega_m = omega_e/pole_pairs
+ * f_c = omega_m*pole_pairs/(2*PI)
+ *
+ * filter = T * (2*PI) * f_c
+ * filter = T * omega_m * pole_pairs
+ *
+ * T - [s] period at which the digital filter is being
+ * calculated
+ * f_in - [Hz] input frequency of the filter
+ * f_c - [Hz] cutoff frequency of the filter
+ * omega_m - [rad/s] mechanical angular velocity
+ * omega_e - [rad/s] electrical angular velocity
+ * pole_pairs - pole pairs
+ *
+ */
+
+ filter = b16mulb16(b16mulb16(o->per, speed), itob16(phy->p));
+
+ /* Limit SMO filters
+ * REVISIT: lowest filter limit should depend on minimum speed:
+ * filter = T * (2*PI) * f_c = T * omega0
+ *
+ */
+
+ if (filter >= b16ONE)
+ {
+ filter = ftob16(0.99f);
+ }
+ else if (filter < ftob16(0.005f))
+ {
+ filter = ftob16(0.005f);
+ }
+
+ smo->emf_lp_filter1 = filter;
+ smo->emf_lp_filter2 = smo->emf_lp_filter1;
+
+ /* Get voltage error: v_err = v_ab - emf */
+
+ v_err->a = v_ab->a - emf->a;
+ v_err->b = v_ab->b - emf->b;
+
+ /* Estimate stator current */
+
+ i_est->a = (b16mulb16(smo->F, i_est->a) +
+ b16mulb16(smo->G, (v_err->a - z->a)));
+ i_est->b = (b16mulb16(smo->F, i_est->b) +
+ b16mulb16(smo->G, (v_err->b - z->b)));
+
+ /* Get motor current error */
+
+ i_err->a = i_ab->a - i_est->a;
+ i_err->b = i_ab->b - i_est->b;
+
+ /* Slide-mode controller */
+
+ sign->a = (i_err->a > 0 ? b16ONE : -b16ONE);
+ sign->b = (i_err->b > 0 ? b16ONE : -b16ONE);
+
+ /* Get current error absolute value - just multiply value with its sign */
+
+ i_err_a_abs = b16mulb16(i_err->a, sign->a);
+ i_err_b_abs = b16mulb16(i_err->b, sign->b);
+
+ /* Calculate new output correction factor voltage */
+
+ if (i_err_a_abs < smo->err_max)
+ {
+ /* Enter linear region if error is small enough */
+
+ z->a = b16mulb16(b16mulb16(i_err->a, smo->k_slide),
+ smo->one_by_err_max);
+ }
+ else
+ {
+ /* Non-linear region */
+
+ z->a = b16mulb16(sign->a, smo->k_slide);
+ }
+
+ if (i_err_b_abs < smo->err_max)
+ {
+ /* Enter linear region if error is small enough */
+
+ z->b = b16mulb16(b16mulb16(i_err->b, smo->k_slide),
+ smo->one_by_err_max);
+ }
+ else
+ {
+ /* Non-linear region */
+
+ z->b = b16mulb16(sign->b, smo->k_slide);
+ }
+
+ /* Filter z to obtain estimated emf */
+
+ LP_FILTER_B16(emf->a, z->a, smo->emf_lp_filter1);
+ LP_FILTER_B16(emf->b, z->b, smo->emf_lp_filter1);
+
+ /* Filter emf one more time before angle stimation */
+
+ LP_FILTER_B16(emf_f->a, emf->a, smo->emf_lp_filter2);
+ LP_FILTER_B16(emf_f->b, emf->b, smo->emf_lp_filter2);
+
+ /* Estimate phase angle according to:
+ * emf_a = -|emf| * sin(th)
+ * emf_b = |emf| * cos(th)
+ * th = atan2(-emf_a, emf->b)
+ *
+ * NOTE: bottleneck but we can't do much more to optimise this
+ */
+
+ angle = fast_atan2_b16(-emf->a, emf->b);
+
+ /* Angle compensation.
+ * Due to low pass filtering we have some delay in estimated phase angle.
+ *
+ * Adaptive filters introduced above cause -PI/4 phase shift for each
+ * filter. We use 2 times filtering which give us constant -PI/2 (-90deg)
+ * phase shift.
+ */
+
+ angle = angle + b16mulb16(dir, b16HALFPI);
+
+ /* Normalize angle to range <0, 2PI> */
+
+ angle_norm_2pi_b16(&angle, 0, b16TWOPI);
+
+ /* Store estimated angle in observer data */
+
+ o->angle = angle;
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_div_init_b16
+ *
+ * Description:
+ * Initialize DIV speed observer
+ *
+ * Input Parameters:
+ * so - (in/out) pointer to the DIV speed observer data
+ * sample - (in) number of angle samples
+ * filter - (in) low-pass filter for final omega
+ * per - (in) speed observer execution period
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_sobserver_div_init_b16(FAR struct motor_sobserver_div_b16_s *so,
+ uint8_t samples, b16_t filter, b16_t per)
+{
+ LIBDSP_DEBUGASSERT(so != NULL);
+ LIBDSP_DEBUGASSERT(samples > 0);
+ LIBDSP_DEBUGASSERT(filter > 0);
+
+ /* Reset observer data */
+
+ memset(so, 0, sizeof(struct motor_sobserver_div_b16_s));
+
+ /* Store number of samples for DIV observer */
+
+ so->samples = samples;
+
+ /* Store low-pass filter for DIV observer speed */
+
+ so->filter = filter;
+
+ /* Store inverted sampling period */
+
+ so->one_by_dt = b16divb16(b16ONE, b16muli(per, so->samples));
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_div_b16
+ *
+ * Description:
+ * Estimate motor speed based on motor angle difference (electrical
+ * or mechanical)
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the speed observer data
+ * angle - (in) angle normalized to <0.0, 2PI>
+ * dir - (in) rotation direction. Valid values:
+ * DIR_CW (1.0f) or DIR_CCW(-1.0f)
+ *
+ ****************************************************************************/
+
+void motor_sobserver_div_b16(FAR struct motor_sobserver_b16_s *o,
+ b16_t angle)
+{
+ LIBDSP_DEBUGASSERT(o != NULL);
+ LIBDSP_DEBUGASSERT(angle >= 0 && angle <= b16TWOPI);
+
+ FAR struct motor_sobserver_div_b16_s *so =
+ (FAR struct motor_sobserver_div_b16_s *)o->so;
+ volatile b16_t omega = 0;
+
+ LIBDSP_DEBUGASSERT(so != NULL);
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi_b16(&angle, -b16PI, b16PI);
+
+ /* Get angle diff */
+
+ so->angle_diff = angle - so->angle_prev;
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi_b16(&so->angle_diff, -b16PI, b16PI);
+
+ /* Accumulate angle only if sample is valid */
+
+ so->angle_acc += so->angle_diff;
+
+ /* Increase counter */
+
+ so->cntr += 1;
+
+ /* Accumulate angle until we get configured number of samples */
+
+ if (so->cntr >= so->samples)
+ {
+ /* Estimate omega using accumulated angle samples.
+ * In this case use simple estimation:
+ *
+ * omega = delta_theta/delta_time
+ * speed_now = low_pass(omega)
+ *
+ */
+
+ omega = b16mulb16(so->angle_acc, so->one_by_dt);
+
+ /* Store filtered omega.
+ *
+ * REVISIT: cut-off frequency for this filter should be
+ * (probably) set according to minimum supported omega:
+ *
+ * filter = T * (2*PI) * f_c = T * omega0
+ *
+ * where:
+ * omega0 - minimum angular speed
+ * T - speed estimation period (samples*per)
+ */
+
+ LP_FILTER_B16(o->speed, omega, so->filter);
+
+ /* Reset samples counter and accumulated angle */
+
+ so->cntr = 0;
+ so->angle_acc = 0;
+ }
+
+ /* Store current angle as previous angle */
+
+ so->angle_prev = angle;
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_nfo_init_b16
+ *
+ * Description:
+ * Initialize motor nolinear fluxlink observer.
+ *
+ * Input Parameters:
+ * nfo - pointer to the nolinear fluxlink observer private data
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+void motor_aobserver_nfo_init_b16(FAR struct motor_aobserver_nfo_b16_s *nfo)
+{
+ LIBDSP_DEBUGASSERT(nfo != NULL);
+
+ /* Reset structure */
+
+ memset(nfo, 0, sizeof(struct motor_aobserver_nfo_b16_s));
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_nfo_b16
+ *
+ * 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 angle 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_aobserver_nfo_b16(FAR struct motor_aobserver_b16_s *o,
+ FAR ab_frame_b16_t *i_ab, FAR ab_frame_b16_t *v_ab,
+ FAR struct motor_phy_params_b16_s *phy, b16_t gain)
+{
+ FAR struct motor_aobserver_nfo_b16_s *nfo =
+ (FAR struct motor_aobserver_nfo_b16_s *)o->ao;
+ b16_t angle;
+ b16_t err;
+ b16_t x1_dot;
+ b16_t x2_dot;
+
+ b16_t l_ia = 0;
+ b16_t l_ib = 0;
+ b16_t r_ia = 0;
+ b16_t r_ib = 0;
+
+ LIBDSP_DEBUGASSERT(nfo != NULL);
+
+ l_ia = b16mulb16(b16mulb16((b16ONE + b16HALF), phy->ind), i_ab->a);
+ l_ib = b16mulb16(b16mulb16((b16ONE + b16HALF), phy->ind), i_ab->b);
+ r_ia = b16mulb16(b16mulb16((b16ONE + b16HALF), phy->res), i_ab->a);
+ r_ib = b16mulb16(b16mulb16((b16ONE + b16HALF), phy->res), i_ab->b);
+
+ err = SQ_B16(phy->flux_link) - (SQ_B16(nfo->x1 - l_ia) +
+ SQ_B16(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)
+ {
+ err = 0;
+ }
+
+ x1_dot = -r_ia + v_ab->a + b16mulb16(b16mulb16(gain, (nfo->x1 - l_ia)),
+ err);
+ x2_dot = -r_ib + v_ab->b + b16mulb16(b16mulb16(gain, (nfo->x2 - l_ib)),
+ err);
+ nfo->x1 += b16mulb16(x1_dot, o->per);
+ nfo->x2 += b16mulb16(x2_dot, o->per);
+
+ /* Prevent the magnitude from getting too low
+ * as that makes the angle very unstable.
+ */
+
+ if (vector2d_mag_b16(nfo->x1, nfo->x2) <
+ (b16mulb16(phy->flux_link, b16HALF)))
+ {
+ nfo->x1 = b16mulb16((b16ONE + b16ONETENTH), nfo->x1);
+ nfo->x2 = b16mulb16((b16ONE + b16ONETENTH), nfo->x2);
+ }
+
+ angle = fast_atan2_b16(nfo->x2 - l_ib, nfo->x1 - l_ia);
+
+ /* Normalize angle to range <0, 2PI> */
+
+ angle_norm_2pi_b16(&angle, 0, b16TWOPI);
+
+ /* Store estimated angle in observer data */
+
+ o->angle = angle;
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_pll_init_b16
+ *
+ * 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_b16(FAR struct motor_sobserver_pll_b16_s *so,
+ b16_t pll_kp, b16_t pll_ki)
+{
+ LIBDSP_DEBUGASSERT(so != NULL);
+ LIBDSP_DEBUGASSERT(pll_kp > 0);
+ LIBDSP_DEBUGASSERT(pll_ki > 0);
+
+ /* Reset observer data */
+
+ memset(so, 0, sizeof(struct motor_sobserver_pll_b16_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_b16
+ *
+ * Description:
+ * Estimate motor electrical speed based on motor electrical angle
+ * difference.
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the speed observer data
+ * angle - (in) electrical angle normalized to <0.0, 2PI>
+ *
+ ****************************************************************************/
+
+void motor_sobserver_pll_b16(FAR struct motor_sobserver_b16_s *o,
+ b16_t angle)
+{
+ FAR struct motor_sobserver_pll_b16_s *so =
+ (FAR struct motor_sobserver_pll_b16_s *)o->so;
+ b16_t delta_theta = 0;
+
+ LIBDSP_DEBUGASSERT(so != NULL);
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi_b16(&angle, -b16PI, -b16PI);
+
+ delta_theta = angle - so->pll_phase;
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi_b16(&delta_theta, -b16PI, -b16PI);
+
+ so->pll_phase += b16mulb16((o->speed + b16mulb16(so->pll_kp, delta_theta)),
+ o->per);
+
+ /* Normalize angle to range <-PI, PI> */
+
+ angle_norm_2pi_b16(&so->pll_phase, -b16PI, -b16PI);
+
+ o->speed += b16mulb16(b16mulb16(so->pll_ki, delta_theta), o->per);
+}
+
+/****************************************************************************
+ * Name: motor_sobserver_speed_get_b16
+ *
+ * Description:
+ * Get the estmiated motor speed from the observer
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the speed observer data
+ *
+ * Returned Value:
+ * Return estimated motor speed from observer
+ *
+ ****************************************************************************/
+
+b16_t motor_sobserver_speed_get_b16(FAR struct motor_sobserver_b16_s *o)
+{
+ LIBDSP_DEBUGASSERT(o != NULL);
+
+ return o->speed;
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_angle_get_b16
+ *
+ * Description:
+ * Get the estmiated motor electrical angle from the observer
+ *
+ * Input Parameters:
+ * o - (in/out) pointer to the angle observer data
+ *
+ * Returned Value:
+ * Return estimated motor electrical angle from observer
+ *
+ ****************************************************************************/
+
+b16_t motor_aobserver_angle_get_b16(FAR struct motor_aobserver_b16_s *o)
+{
+ LIBDSP_DEBUGASSERT(o != NULL);
+
+ return o->angle;
+}