nuttx/libs/libdsp/lib_observer.c

/****************************************************************************
 * control/lib_observer.c
 *
 *   Copyright (C) 2018 Gregory Nutt. All rights reserved.
 *   Author: Mateusz Szafoni <raiden00@railab.me>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name NuttX nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <stddef.h>
#include <assert.h>

#include <dsp.h>

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

/****************************************************************************
 * Private Functions
 ****************************************************************************/

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: motor_observer_init
 *
 * Description:
 *   Initialize motor observer
 *
 * Input Parameters:
 *   observer - pointer to the common observer data
 *   ao       - pointer to the angle specific observer data
 *   so       - pointer to the speed specific observer data
 *   per      - observer execution period
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_observer_init(FAR struct motor_observer_s *observer,
                         FAR void *ao, FAR void *so, float per)
{
  DEBUGASSERT(observer != NULL);
  DEBUGASSERT(ao != NULL);
  DEBUGASSERT(so != NULL);
  DEBUGASSERT(per > 0.0);

  /* Set observer period */

  observer->per = per;

  /* Connect angle estimation observer data */

  observer->ao = ao;

  /* Connect speed estimation observer data */

  observer->so = so;
}

/****************************************************************************
 * Name: motor_observer_smo_init
 *
 * 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_observer_smo_init(FAR struct motor_observer_smo_s *smo,
                             float kslide,
                             float err_max)
{
  DEBUGASSERT(smo != NULL);
  DEBUGASSERT(kslide > 0.0);
  DEBUGASSERT(err_max > 0.0);

  /* Initialize structure */

  smo->k_slide = kslide;
  smo->err_max = err_max;
}

/****************************************************************************
 * Name: motor_observer_smo
 *
 * 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:
 *   observer - (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
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i_ab,
                  FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy)
{
  FAR struct motor_observer_smo_s *smo =
    (FAR struct motor_observer_smo_s *)observer->ao;
  FAR ab_frame_t *emf    = &smo->emf;
  FAR ab_frame_t *z      = &smo->z;
  FAR ab_frame_t *i_est  = &smo->i_est;
  FAR ab_frame_t *v_err  = &smo->v_err;
  FAR ab_frame_t *i_err  = &smo->i_err;
  FAR ab_frame_t *sign   = &smo->sign;
  float i_err_a_abs  = 0.0;
  float i_err_b_abs  = 0.0;
  float angle        = 0.0;

  /* REVISIT: observer works only when IQ current is high enough */

  /* Calculate observer gains */

  smo->F_gain = (1 - observer->per*phy->res/phy->ind);
  smo->G_gain = observer->per/phy->ind;

  /* Saturate F gain */

  if (smo->F_gain < 0)
    {
      smo->F_gain = 0.0;
    }

  /* Saturate G gain */

  if (smo->G_gain > 0.999)
    {
      smo->G_gain = 0.999;
    }

  /* 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) = -PI/2
   *
   * 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/poles
   *   f_c = omega_m*poles/(2*PI)
   *
   *   filter = T * (2*PI) * f_c
   *   filter = T * omega_m * poles
   *
   *   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
   *
   */

  smo->emf_lp_filter1 = observer->per * observer->speed * phy->poles;
  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 = smo->F_gain * i_est->a + smo->G_gain * (v_err->a - z->a);
  i_est->b = smo->F_gain * i_est->b + smo->G_gain * (v_err->b - z->b);

  /* Get motor current errror */

  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 ? 1.0 : -1.0);
  sign->b = (i_err->b > 0 ? 1.0 : -1.0);

  /* Get current error absolute value - just multiply value with its sign */

  i_err_a_abs = i_err->a * sign->a;
  i_err_b_abs = 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 = i_err->a * smo->k_slide / smo->err_max;
    }
  else
    {
      /* Non-linear region */

      z->a = sign->a * smo->k_slide;
    }

  if (i_err_b_abs < smo->err_max)
    {
      /* Enter linear region if error is small enough */

      z->b = i_err->b * smo->k_slide / smo->err_max;
    }
  else
    {
      /* Non-linear region */

      z->b = sign->b * smo->k_slide;
    }

  /* Filter z to obtain estimated emf */

  LP_FILTER(emf->a, z->a, smo->emf_lp_filter1);
  LP_FILTER(emf->b, z->b, smo->emf_lp_filter1);

  /* Filter emf one more time before angle stimation */

  LP_FILTER(emf->a, emf->a, smo->emf_lp_filter2);
  LP_FILTER(emf->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)
   */

  angle = fast_atan2(-emf->a, emf->b);

#if 1
  /* Some assertions
   * TODO: simplify
   */

  if (angle != angle) angle = 0.0;
  if (emf->a != emf->a) emf->a = 0.0;
  if (emf->b != emf->b) emf->b = 0.0;
  if (z->a != z->a) z->a = 0.0;
  if (z->b != z->b) z->b = 0.0;
  if (i_est->a != i_est->a) i_est->a = 0.0;
  if (i_est->b != i_est->b) i_est->b = 0.0;
#endif

  /* Angle compensation.
   * Due to low pass filtering we have some delay in estimated phase angle.
   *
   * Adaptive filters introduced above cause -PI/2 phase shift for each filter.
   * We use 2 times filtering which give us constant -PI phase shift.
   */

  angle = angle -M_PI_F;

  /* Normalize angle to range <0, 2PI> */

  angle_norm_2pi(&angle, 0.0, 2*M_PI_F);

  /* Store estimated angle in observer data*/

  observer->angle = angle;
}