nuttx-apps/examples/dsptest/test_observer.c

/****************************************************************************
 * examples/dsptest/test_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 "dsptest.h"

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

/****************************************************************************
 * Private Types
 ****************************************************************************/

/* Dummy angle observer */

struct motor_observer_dummy_s
{
  float x;
};

/* Dummy speed observer */

struct motor_sobserver_dummy_s
{
  float x;
};

/****************************************************************************
 * Private Function Protototypes
 ****************************************************************************/

/****************************************************************************
 * Private Data
 ****************************************************************************/

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

/* Initialize observer data */

static void test_observer_init(void)
{
  struct motor_observer_dummy_s  ao;
  struct motor_sobserver_dummy_s so;
  struct motor_observer_s        o;
  float per = 1e-11;

  motor_observer_init(&o, (void*)&ao, (void*)&so, per);

  TEST_ASSERT_EQUAL_FLOAT(0.0, o.angle);
  TEST_ASSERT_EQUAL_FLOAT(0.0, o.speed);
  TEST_ASSERT_EQUAL_FLOAT(per, o.per);
  TEST_ASSERT_NOT_NULL(o.ao);
  TEST_ASSERT_NOT_NULL(o.so);
}

/* Initialize SMO observer data */

static void test_observer_smo_init(void)
{
  struct motor_observer_smo_s ao;
  float kslide  = 0.99;
  float err_max = 0.99;

  /* Initialize SMO observer */

  motor_observer_smo_init(&ao, kslide, err_max);

  /* Test */

  TEST_ASSERT_EQUAL_FLOAT(kslide, ao.k_slide);
  TEST_ASSERT_EQUAL_FLOAT(err_max, ao.err_max);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.F_gain);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.G_gain);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf_lp_filter1);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf_lp_filter2);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf.b);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.z.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.z.b);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_est.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_est.b);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.v_err.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.v_err.b);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_err.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_err.b);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.sign.a);
  TEST_ASSERT_EQUAL_FLOAT(0.0, ao.sign.b);
}

/* Feed SMO observer with zeros */

static void test_observer_smo_zeros(void)
{
  struct motor_sobserver_dummy_s so;
  struct motor_observer_smo_s    ao;
  struct motor_observer_s        o;
  struct motor_phy_params_s      phy;
  ab_frame_t i_ab    = {0.0, 0.0};
  ab_frame_t v_ab    = {0.0, 0.0};
  float      k_slide = 1.0;
  float      err_max = 1.0;
  float      per     = 1e-6;
  float      angle   = 0.0;
  int        i       = 0;

  /* Initialize motor physical parameters */

  phy.p   = 8;
  phy.res = 0.001;
  phy.ind = 10e-6;

  /* Initialize SMO angle observer */

  motor_observer_smo_init(&ao, k_slide, err_max);

  /* Initialize observer */

  motor_observer_init(&o, &ao, &so, per);

  /* Feed SMO observer with zeros. This should return
   * angle equal to observer phase shift (direction * PI)
   */

  i_ab.a = 0.0;
  i_ab.b = 0.0;
  v_ab.a = 0.0;
  v_ab.b = 0.0;

  for (i = 0; i< 1000000; i+=1)
    {
      motor_observer_smo(&o, &i_ab, &v_ab, &phy, DIR_CW);
    }

  angle = motor_observer_angle_get(&o);

  /* Test */

  TEST_ASSERT_EQUAL_FLOAT(M_PI_F, angle);

  for (i = 0; i< 1000000; i+=1)
    {
      motor_observer_smo(&o, &i_ab, &v_ab, &phy, DIR_CCW);
    }

  angle = motor_observer_angle_get(&o);

  /* Test. NOTE: -M_PI_F normalised = 0.0 */

  TEST_ASSERT_FLOAT_WITHIN(5e-6, 0.0, angle);
}

/* Feed SMO in CW direction */

static void test_observer_smo_cw(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

/* Feed SMO in CCW direction */

static void test_observer_smo_ccw(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

/* SMO observer in linear region */

static void test_observer_smo_linear(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

/* SMO observer gain saturation */

static void test_observer_smo_gain_saturate(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

/* DIV speed observer initialization */

static void test_sobserver_div_init(void)
{
  struct motor_sobserver_div_s so;
  uint8_t samples = 10;
  float filter    = 0.1;
  float per       = 10e-6;

  /* Initialize DIV speed observer */

  motor_sobserver_div_init(&so, samples, filter, per);

  TEST_ASSERT_EQUAL_FLOAT(filter, so.filter);
  TEST_ASSERT_EQUAL_FLOAT(1.0/(samples*per), so.one_by_dt);
  TEST_ASSERT_EQUAL_UINT8(samples, so.samples);
}

/* Feed DIV speed observer with zeros */

static void test_sobserver_div_zeros(void)
{
  struct motor_sobserver_div_s  so;
  struct motor_observer_s       o;
  struct motor_observer_dummy_s ao;
  uint8_t samples    = 10;
  float   filter     = 0.1;
  int     i          = 0;
  float   expected_s = 0.0;
  float   speed      = 0.0;
  float   per        = 1e-6;

  /* Initialize DIV speed observer */

  motor_sobserver_div_init(&so, samples, filter, per);

  /* Initialize observer */

  motor_observer_init(&o, &ao, &so, per);

  /* Feed observer with zeros in CW direction */

  expected_s = 0.0;

  for(i = 0; i < 1000; i += 1)
    {
      motor_sobserver_div(&o, 0.0, DIR_CW);
    }

  speed = motor_observer_speed_get(&o);

  /* Test */

  TEST_ASSERT_EQUAL_FLOAT(expected_s, speed);

  /* Feed observer with zeros in CCW direction */

  expected_s = 0.0;

  for(i = 0; i < 1000; i += 1)
    {
      motor_sobserver_div(&o, 0.0, DIR_CCW);
    }

  speed = motor_observer_speed_get(&o);

  /* Test */

  TEST_ASSERT_EQUAL_FLOAT(expected_s, speed);
}

/* Feed DIV speed observer in CW direction */

static void test_sobserver_div_cw(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

/* Feed DIV speed observer in CCW direction */

static void test_sobserver_div_ccw(void)
{
  /* TODO */

  TEST_FAIL_MESSAGE("not implemented");
}

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

/****************************************************************************
 * Name: test_observer
 ****************************************************************************/

void test_observer(void)
{
  UNITY_BEGIN();

  TEST_SEPARATOR();

  /* Test observer functions */

  RUN_TEST(test_observer_init);

  /* SMO observer */

  RUN_TEST(test_observer_smo_init);
  RUN_TEST(test_observer_smo_zeros);
  RUN_TEST(test_observer_smo_linear);
  RUN_TEST(test_observer_smo_gain_saturate);
  RUN_TEST(test_observer_smo_cw);
  RUN_TEST(test_observer_smo_ccw);

  /* DIV observer */

  RUN_TEST(test_sobserver_div_init);
  RUN_TEST(test_sobserver_div_zeros);
  RUN_TEST(test_sobserver_div_cw);
  RUN_TEST(test_sobserver_div_ccw);

  UNITY_END();
}