diff --git a/examples/README.txt b/examples/README.txt index 32d77fc2c..b38119fe0 100644 --- a/examples/README.txt +++ b/examples/README.txt @@ -338,6 +338,22 @@ examples/djoystick CONFIG_EXAMPLES_DJOYSTICK_SIGNO - Signal used to signal the test application. Default 13. + +examples/dsptest +^^^^^^^^^^^^^^^^^^ + + This is a Unit Test for the Nuttx DSP library. It use Unity testing framwork. + + Dependencies: + + CONFIG_LIBDSP=y + CONFIG_LIBDSP_DEBUG=y + CONFIG_TESTING_UNITY=y + + Optional configuration: + + CONFIG_TESTING_UNITY_OUTPUT_COLOR - enable colored output + examples/elf ^^^^^^^^^^^^ diff --git a/examples/dsptest/.gitignore b/examples/dsptest/.gitignore new file mode 100644 index 000000000..fa1ec7579 --- /dev/null +++ b/examples/dsptest/.gitignore @@ -0,0 +1,11 @@ +/Make.dep +/.depend +/.built +/*.asm +/*.obj +/*.rel +/*.lst +/*.sym +/*.adb +/*.lib +/*.src diff --git a/examples/dsptest/Kconfig b/examples/dsptest/Kconfig new file mode 100644 index 000000000..eaaf73df1 --- /dev/null +++ b/examples/dsptest/Kconfig @@ -0,0 +1,30 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config EXAMPLES_DSPTEST + bool "LIBDSP library testing" + default n + ---help--- + Enable the LIBDSP library testing + +if EXAMPLES_DSPTEST + +config EXAMPLES_DSPTEST_PROGNAME + string "Program name" + default "dsptest" + depends on BUILD_KERNEL + ---help--- + This is the name of the program that will be use when the NSH ELF + program is installed. + +config EXAMPLES_DSPTEST_PRIORITY + int "Dsptest task priority" + default 100 + +config EXAMPLES_DSPTEST_STACKSIZE + int "Dsptest stack size" + default 2048 + +endif diff --git a/examples/dsptest/Make.defs b/examples/dsptest/Make.defs new file mode 100644 index 000000000..cc9f862d8 --- /dev/null +++ b/examples/dsptest/Make.defs @@ -0,0 +1,39 @@ +############################################################################ +# apps/examples/dsptest/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2018 Gregory Nutt. All rights reserved. +# Author: Mateusz Szafoni +# +# 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. +# +############################################################################ + +ifeq ($(CONFIG_EXAMPLES_DSPTEST),y) +CONFIGURED_APPS += examples/dsptest +endif diff --git a/examples/dsptest/Makefile b/examples/dsptest/Makefile new file mode 100644 index 000000000..557b97427 --- /dev/null +++ b/examples/dsptest/Makefile @@ -0,0 +1,59 @@ +############################################################################ +# apps/examples/dsptest/Makefile +# +# Copyright (C) 2018 Gregory Nutt. All rights reserved. +# Author: Mateusz Szafoni +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +# dsptest built-in application info + +CONFIG_EXAMPLES_DSPTEST_PRIORITY ?= SCHED_PRIORITY_DEFAULT +CONFIG_EXAMPLES_DSPTEST_STACKSIZE ?= 2048 + +APPNAME = dsptest +PRIORITY = $(CONFIG_EXAMPLES_DSPTEST_PRIORITY) +STACKSIZE = $(CONFIG_EXAMPLES_DSPTEST_STACKSIZE) + +# dsptest example + +ASRCS = +CSRCS = +MAINSRC = dsptest_main.c + +CSRCS += test_foc.c test_pid.c test_svm.c test_misc.c test_motor.c +CSRCS += test_observer.c test_transform.c + +CONFIG_EXAMPLES_DSPTEST_PROGNAME ?= dsptest$(EXEEXT) +PROGNAME = $(CONFIG_EXAMPLES_DSPTEST_PROGNAME) + +include $(APPDIR)/Application.mk diff --git a/examples/dsptest/dsptest.h b/examples/dsptest/dsptest.h new file mode 100644 index 000000000..4c5b443eb --- /dev/null +++ b/examples/dsptest/dsptest.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * apps/examples/dsptest/dsptest.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_DSPTEST_DSPTEST_H +#define __APPS_EXAMPLES_DSPTEST_DSPTEST_H + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define TEST_SINCOS_DELTA 0.06 +#define TEST_SINCOS2_DELTA 0.01 +#define TEST_ATAN2_DELTA 0.015 + +#define TEST_SEPARATOR() \ + printf("\n========================================================\n"); \ + printf("-> %s\n", __FILE__); \ + printf("========================================================\n\n"); + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* test_misc.c **************************************************************/ + +void test_misc(void); + +/* test_pid.c ***************************************************************/ + +void test_pid(void); + +/* test_transform.c *********************************************************/ + +void test_transform(void); + +/* test_foc.c ***************************************************************/ + +void test_foc(void); + +/* test_svm.c ***************************************************************/ + +void test_svm(void); + +/* test_observer.c **********************************************************/ + +void test_observer(void); + +/* test_motor.c *************************************************************/ + +void test_motor(void); + +#endif /* __APPS_EXAMPLES_DSPTEST_DSPTEST_H */ diff --git a/examples/dsptest/dsptest_main.c b/examples/dsptest/dsptest_main.c new file mode 100644 index 000000000..db2fc4e03 --- /dev/null +++ b/examples/dsptest/dsptest_main.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * examples/dsptest/dsptest_main.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +#ifndef CONFIG_TESTING_UNITY +# error "This example needs Unity testing library" +#endif + +#ifndef CONFIG_LIBDSP +# error "Libdsp not enabled" +#endif + +#ifndef CONFIG_LIBDSP_DEBUG +# error "Libdsp debug not enabled" +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * dsptest_main + ****************************************************************************/ + +#ifdef CONFIG_BUILD_KERNEL +int main(int argc, FAR char *argv[]) +#else +int dsptest_main(int argc, char *argv[]) +#endif +{ + /* Test misc module */ + + test_misc(); + + /* Test pid module */ + + test_pid(); + + /* Test transform module */ + + test_transform(); + + /* Test foc module */ + + test_foc(); + + /* Test SVM module */ + + test_svm(); + + /* Test observer module */ + + test_observer(); + + /* Test motor module */ + + test_motor(); + + return 0; +} diff --git a/examples/dsptest/test_foc.c b/examples/dsptest/test_foc.c new file mode 100644 index 000000000..6a10bc1cb --- /dev/null +++ b/examples/dsptest/test_foc.c @@ -0,0 +1,493 @@ +/**************************************************************************** + * examples/dsptest/test_foc.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +/* Set float precision for this module */ + +#undef UNITY_FLOAT_PRECISION + +#if CONFIG_LIBDSP_PRECISION == 1 +# define UNITY_FLOAT_PRECISION (0.1f) +#elif CONFIG_LIBDSP_PRECISION == 2 +# define UNITY_FLOAT_PRECISION (0.001f) +#else +# define UNITY_FLOAT_PRECISION (0.999f) +#endif + +/* Value close enough to zero */ + +#define TEST_ASSERT_EQUAL_FLOAT_ZERO(a) \ + TEST_ASSERT_FLOAT_WITHIN(1e-6, 0.0, a); + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Initialize FOC data */ + +static void test_foc_init(void) +{ + struct foc_data_s foc; + float id_kp = 1.0; + float id_ki = 2.0; + float iq_kp = 3.0; + float iq_ki = 4.0; + + foc_init(&foc, id_kp, id_ki, iq_kp, iq_ki); + + TEST_ASSERT_EQUAL_FLOAT(1.0, foc.id_pid.KP); + TEST_ASSERT_EQUAL_FLOAT(2.0, foc.id_pid.KI); + TEST_ASSERT_EQUAL_FLOAT(0.0, foc.id_pid.KD); + TEST_ASSERT_EQUAL_FLOAT(3.0, foc.iq_pid.KP); + TEST_ASSERT_EQUAL_FLOAT(4.0, foc.iq_pid.KI); + TEST_ASSERT_EQUAL_FLOAT(0.0, foc.iq_pid.KD); + + foc_idq_ref_set(&foc, 1.0, 2.0); + + TEST_ASSERT_EQUAL_FLOAT(1.0, foc.i_dq_ref.d); + TEST_ASSERT_EQUAL_FLOAT(2.0, foc.i_dq_ref.q); + + foc_vbase_update(&foc, 0.1); + + TEST_ASSERT_EQUAL_FLOAT(0.1, 1.0/foc.vab_mod_scale); + TEST_ASSERT_EQUAL_FLOAT(0.1, foc.vdq_mag_max); +} + +/* Feed FOC with zeros */ + +static void test_foc_process_zeros(void) +{ + struct foc_data_s foc; + float id_kp = 1.00; + float id_ki = 0.01; + float iq_kp = 1.00; + float iq_ki = 0.01; + abc_frame_t i_abc; + phase_angle_t angle; + + /* Initialize FOC */ + + foc_init(&foc, id_kp, id_ki, iq_kp, iq_ki); + + /* Input: + * abc = (0.0, 0.0, 0.0) + * i_dq_ref = (1.0, 1.0) + * vab_scale = 1.0 + * vdq_mag_max = 1.0 + * angle = 0.0 + * + * Expected: + * i_ab = (0.0, 0.0) + * i_dq = (0.0, 0.0) + * v_dq = (0.0, 0.0) + * v_ab = (0.0, 0.0) + * v_ab_mod = (0.0, 0.0) + */ + + i_abc.a = 0.0; + i_abc.b = 0.0; + i_abc.c = 0.0; + phase_angle_update(&angle, 0.0); + foc_idq_ref_set(&foc, 0.0, 0.0); + foc_vbase_update(&foc, 1.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, -0.5, -0.5) + * i_dq_ref = (1.0, 0.0) + * vab_scale = 1.0 + * vdq_mag_max = 1.0 + * angle = 0.0 + * + * Expected: + * i_ab = (1.0, 0.0) + * i_dq = (1.0, 0.0) + * v_dq = (0.0, 0.0) + * v_ab = (0.0, 0.0) + * v_ab_mod = (0.0, 0.0) + */ + + i_abc.a = 1.0; + i_abc.b = -0.5; + i_abc.c = -0.5; + foc_idq_ref_set(&foc, 1.0, 0.0); + foc_vbase_update(&foc, 1.0); + phase_angle_update(&angle, 0.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.b); + + /* Input: + * abc = (-1.0, 0.5, 0.5) + * i_dq_ref = (-1.0, 0.0) + * vab_scale = 1.0 + * vdq_mag_max = 1.0 + * angle = 0.0 + * + * Expected: + * i_ab = (-1.0, 0.0) + * i_dq = (-1.0, 0.0) + * v_dq = (0.0, 0.0) + * v_ab = (0.0, 0.0) + * v_ab_mod = (0.0, 0.0) + */ + + i_abc.a = -1.0; + i_abc.b = 0.5; + i_abc.c = 0.5; + foc_idq_ref_set(&foc, -1.0, 0.0); + foc_vbase_update(&foc, 1.0); + phase_angle_update(&angle, 0.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT_ZERO(foc.v_ab_mod.b); +} + +/* Proces FOC with some test data */ + +static void test_foc_process(void) +{ + struct foc_data_s foc; + float id_kp = 1.00; + float id_ki = 0.00; + float iq_kp = 1.00; + float iq_ki = 0.00; + abc_frame_t i_abc; + phase_angle_t angle; + float i_d_ref = 0.0; + float i_q_ref = 0.0; + + /* Initialize FOC. + * For simplicity KP = 1.0, KI = 0.0 + */ + + foc_init(&foc, id_kp, id_ki, iq_kp, iq_ki); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (2.0, 2.0) + * vab_scale = 0.1 + * vdq_mag_max = 10.0 + * angle = PI + * + * Expected: + * i_ab = (1.0, 1.7320) + * i_dq = (-1.0, -1.7320) + * v_dq = (3.0, 3.7320) + * v_ab = (0.30, 0.37320) + * v_ab_mod = (0.30, 0.37320) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = 2.0; + i_q_ref = 2.0; + phase_angle_update(&angle, M_PI_F); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0 , foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320 , foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-1.0 , foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(-1.7320, foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(3.0 , foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(3.7320 , foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-3.0 , foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(-3.7320, foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-0.3 , foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(-0.3732, foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (2.0, 2.0) + * vab_scale = 0.1 + * vdq_mag_max = 10 + * angle = -PI + * + * Expected: + * i_ab = (1.0, 1.7320) + * i_dq = (-1.0, -1.7320) + * v_dq = (3.0, 3.7320) + * v_ab = (-0.30, -0.37320) + * v_ab_mod = (-0.30, -0.37320) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = 2.0; + i_q_ref = 2.0; + phase_angle_update(&angle, -M_PI_F); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0 , foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320 , foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-1.0 , foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(-1.7320, foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(3.0 , foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(3.7320 , foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-3.0 , foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(-3.7320, foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-0.30 , foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(-0.3732, foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (2.0, 2.0) + * vab_scale = 0.1 + * vdq_mag_max = 10 + * angle = 0.1 + * + * Expected: + * i_ab = (1.0, 1.7320) + * i_dq = (1.1679, 1.6236) + * v_dq = (0.8321, 0.3764) + * v_ab = (0.7903, 0.4576) + * v_ab_mod = (0.07903, 0.04576) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = 2.0; + i_q_ref = 2.0; + phase_angle_update(&angle, 0.1); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0 , foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320 , foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(1.1679 , foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.6236 , foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(0.8321 , foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(0.3764 , foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(0.7903 , foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(0.4576 , foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(0.07903, foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(0.04576, foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (2.0, 2.0) + * vab_scale = 0.1 + * vdq_mag_max = 10.0 + * angle = -0.1 + * + * Expected: + * i_ab = (1.0 , 1.7320) + * i_dq = (0.8221, 1.8232) + * v_dq = (1.1779, 0.1768) + * v_ab = (0.11897, 0.00583) + * v_ab_mod = (0.11897, 0.00583) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = 2.0; + i_q_ref = 2.0; + phase_angle_update(&angle, -0.1); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0, foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320, foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(0.8221, foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.8232, foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(1.1779, foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(0.1768, foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(1.1897, foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(0.0583, foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(0.11897, foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(0.00583, foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (-2.0, 0.0) + * vab_scale = 0.1 + * vdq_mag_max = 10.0 + * angle = 0.1 + * + * Expected: + * i_ab = (1.0, 1.7320) + * i_dq = (1.1679, 1.6236) + * v_dq = (-3.1679, -1.6236) + * v_ab = (-0.29900, -0.19317) + * v_ab_mod = (-0.29900, -0.19317) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = -2.0; + i_q_ref = 0.0; + phase_angle_update(&angle, 0.1); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0 , foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320 , foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(1.1679 , foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.6236 , foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-3.1679 , foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(-1.6236 , foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-2.9900 , foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(-1.9317 , foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-0.29900 , foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(-0.19317 , foc.v_ab_mod.b); + + /* Input: + * abc = (1.0, 1.0, -2.0) + * i_dq_ref = (-2.0, -2.0) + * vab_scale = 0.1 + * vdq_mag_max = 10.0 + * angle = 0.1 + * + * Expected: + * i_ab = (1.0, 1.7320) + * i_dq = (1.1679, 1.6236) + * v_dq = (-3.1679, -3.6236) + * v_ab = (-0.27903, -0.39217) + * v_ab_mod = (-0.27903, -0.39217) + */ + + i_abc.a = 1.0; + i_abc.b = 1.0; + i_abc.c = -2.0; + i_d_ref = -2.0; + i_q_ref = -2.0; + phase_angle_update(&angle, 0.1); + foc_idq_ref_set(&foc, i_d_ref, i_q_ref); + foc_vbase_update(&foc, 10.0); + + foc_process(&foc, &i_abc, &angle); + + TEST_ASSERT_EQUAL_FLOAT(1.0 , foc.i_ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320 , foc.i_ab.b); + TEST_ASSERT_EQUAL_FLOAT(1.1679 , foc.i_dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.6236 , foc.i_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-3.1679 , foc.v_dq.d); + TEST_ASSERT_EQUAL_FLOAT(-3.6236 , foc.v_dq.q); + TEST_ASSERT_EQUAL_FLOAT(-2.7903 , foc.v_ab.a); + TEST_ASSERT_EQUAL_FLOAT(-3.9217 , foc.v_ab.b); + TEST_ASSERT_EQUAL_FLOAT(-0.27903, foc.v_ab_mod.a); + TEST_ASSERT_EQUAL_FLOAT(-0.39217, foc.v_ab_mod.b); +} + +/* Test FOC saturation */ + +static void test_foc_saturation(void) +{ + TEST_FAIL_MESSAGE("not implemented"); +} + +/* Test FOC vdq modulation */ + +static void test_foc_vdq_modulation(void) +{ + TEST_FAIL_MESSAGE("not implemented"); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_foc + ****************************************************************************/ + +void test_foc(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test FOC */ + + RUN_TEST(test_foc_init); + RUN_TEST(test_foc_process_zeros); + RUN_TEST(test_foc_process); + RUN_TEST(test_foc_saturation); + RUN_TEST(test_foc_vdq_modulation); + + UNITY_END(); +} diff --git a/examples/dsptest/test_misc.c b/examples/dsptest/test_misc.c new file mode 100644 index 000000000..ba0ec7388 --- /dev/null +++ b/examples/dsptest/test_misc.c @@ -0,0 +1,633 @@ +/**************************************************************************** + * examples/dsptest/test_misc.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +/* Set float precision for this module */ + +#undef UNITY_FLOAT_PRECISION +#define UNITY_FLOAT_PRECISION (0.0001f) + +#define TEST_SINCOS_ANGLE_STEP 0.01 +#define TEST_SINCOS2_ANGLE_STEP 0.01 +#define TEST_ATAN2_XY_STEP 0.01 + +/* Angle sin/cos depends on libdsp precision */ + +#if CONFIG_LIBDSP_PRECISION == 1 +# define ANGLE_SIN(val) fast_sin2(val) +# define ANGLE_COS(val) fast_cos2(val) +#elif CONFIG_LIBDSP_PRECISION == 2 +# define ANGLE_SIN(val) sin(val) +# define ANGLE_COS(val) cos(val) +#else +# define ANGLE_SIN(val) fast_sin(val) +# define ANGLE_COS(val) fast_cos(val) +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Test float value saturation */ + +static void test_f_saturate(void) +{ + float val = 0.0; + + /* Value in the range */ + + val = 0.9; + f_saturate(&val, 0.0, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(0.9, val); + + /* Upper limit */ + + val = 1.2; + f_saturate(&val, 0.0, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(1.0, val); + + /* Lower limit */ + + val = 0.0; + f_saturate(&val, 0.1, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(0.1, val); +} + +/* Test 2D vector magnitude */ + +static void test_vector2d_mag(void) +{ + float mag = 0.0; + + /* mag([0.0, 0.0]) = 0.0 */ + + mag = vector2d_mag(0.0, 0.0); + + TEST_ASSERT_EQUAL_FLOAT(0.0, mag); + + /* mag([1.0, 1.0]) = 1.4142 */ + + mag = vector2d_mag(1.0, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(1.4142, mag); + + /* mag([-1.0, 1.0]) = 1.4142 */ + + mag = vector2d_mag(-1.0, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(1.4142, mag); +} + +/* Test 2D vector saturation */ + +static void test_vector2d_saturate(void) +{ + float x = 0.0; + float y = 0.0; + float mag = 0.0; + float mag_ref = 0.0; + + /* Vector magnitude 0.0 */ + + x = 0.0; + y = 0.0; + mag_ref = 1.0; + vector2d_saturate(&x, &y, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(0.0, x); + TEST_ASSERT_EQUAL_FLOAT(0.0, y); + + /* Vector magnitude 0.0, saturation 0.0 */ + + x = 0.0; + y = 0.0; + mag_ref = 0.0; + vector2d_saturate(&x, &y, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(mag_ref, x); + TEST_ASSERT_EQUAL_FLOAT(mag_ref, y); + + /* Vector magnitude 1.4142, saturation 0.0 */ + + x = 1.0; + y = 1.0; + mag_ref = 0.0; + vector2d_saturate(&x, &y, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(0.0, x); + TEST_ASSERT_EQUAL_FLOAT(0.0, y); + + /* Vector magnitude 1.4142, saturation 3.0 */ + + x = 1.0; + y = 1.0; + mag_ref = 3.0; + vector2d_saturate(&x, &y, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(1.0, x); + TEST_ASSERT_EQUAL_FLOAT(1.0, y); + + /* Vector magnitude 1.4142, saturation 1.0 - truncate */ + + x = 1.0; + y = 1.0; + mag_ref = 1.0; + vector2d_saturate(&x, &y, mag_ref); + mag = vector2d_mag(x, y); + + TEST_ASSERT_EQUAL_FLOAT(mag_ref, mag); +} + +/* Test dq vector magnitude */ + +static void test_dq_mag(void) +{ + TEST_IGNORE_MESSAGE("test_dq_mag not implemented"); +} + +/* Test dq vector saturation */ + +static void test_dq_saturate(void) +{ + dq_frame_t dq; + float mag_ref = 0.0; + float mag = 0.0; + + /* Vector magnitude 0.0 */ + + dq.d = 0.0; + dq.q = 0.0; + mag_ref = 1.0; + dq_saturate(&dq, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(0.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(0.0, dq.q); + + /* Vector magnitude 0.0, saturation 0.0 */ + + dq.d = 0.0; + dq.q = 0.0; + mag_ref = 0.0; + dq_saturate(&dq, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(mag_ref, dq.d); + TEST_ASSERT_EQUAL_FLOAT(mag_ref, dq.q); + + /* Vector magnitude 1.4142, saturation 0.0 */ + + dq.d = 1.0; + dq.q = 1.0; + mag_ref = 0.0; + dq_saturate(&dq, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(mag_ref, dq.d); + TEST_ASSERT_EQUAL_FLOAT(mag_ref, dq.q); + + /* Vector magnitude 1.4142, saturation 3.0 */ + + dq.d = 1.0; + dq.q = 1.0; + mag_ref = 3.0; + dq_saturate(&dq, mag_ref); + + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.q); + + /* Vector magnitude 1.4142, saturation 1.0 - truncate */ + + dq.d = 1.0; + dq.q = 1.0; + mag_ref = 1.0; + dq_saturate(&dq, mag_ref); + mag = dq_mag(&dq); + + TEST_ASSERT_EQUAL_FLOAT(mag_ref, mag); +} + +/* Test fast sine */ + +static void test_fast_sin(void) +{ + float s_ref = 0.0; + float angle = 0.0; + float s = 0.0; + + /* Compare with LIBC sine */ + + for(angle = 0.0; angle < 2*M_PI_F; angle += TEST_SINCOS_ANGLE_STEP) + { + s_ref = sinf(angle); + s = fast_sin(angle); + + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s_ref, s); + } +} + +/* Test fast cosine */ + +static void test_fast_cos(void) +{ + float c_ref = 0.0; + float angle = 0.0; + float c = 0.0; + + /* Compare with LIBC cosine */ + + for(angle = 0.0; angle < 2*M_PI_F; angle += TEST_SINCOS_ANGLE_STEP) + { + c_ref = cosf(angle); + c = fast_cos(angle); + + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c_ref, c); + } +} + +/* Test fast sine (better accuracy) */ + +static void test_fast_sin2(void) +{ + float s_ref = 0.0; + float angle = 0.0; + float s = 0.0; + + /* Compare with LIBC sine */ + + for(angle = 0.0; angle < 2*M_PI_F; angle += TEST_SINCOS2_ANGLE_STEP) + { + s_ref = sinf(angle); + s = fast_sin2(angle); + + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS2_DELTA, s_ref, s); + } +} + +/* Test fast cosine (better accuracy) */ + +static void test_fast_cos2(void) +{ + float c_ref = 0.0; + float angle = 0.0; + float c = 0.0; + + /* Compare with LIBC cosine */ + + for(angle = 0.0; angle < 2*M_PI_F; angle += TEST_SINCOS2_ANGLE_STEP) + { + c_ref = cosf(angle); + c = fast_cos2(angle); + + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS2_DELTA, c_ref, c); + } +} + +/* Test fast atan2 */ + +static void test_fast_atan2(void) +{ + float angle_ref = 0.0; + float angle = 0.0; + float x = 0.0; + float y = 0.0; + + /* atan2(0, 0) - special case when atan2 is not defined */ + + angle = fast_atan2(y, x); + + /* Expect non inf and non nan */ + + TEST_ASSERT_FLOAT_IS_DETERMINATE(angle); + + /* Compare with LIBC atan2 */ + + for (x = TEST_ATAN2_XY_STEP; x < M_PI_F; x += TEST_ATAN2_XY_STEP) + { + for (y = TEST_ATAN2_XY_STEP; y < M_PI_F; y += TEST_ATAN2_XY_STEP) + { + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + } + } + + for (x = -TEST_ATAN2_XY_STEP; x > -M_PI_F; x -= TEST_ATAN2_XY_STEP) + { + for (y = -TEST_ATAN2_XY_STEP; y > -M_PI_F; y -= TEST_ATAN2_XY_STEP) + { + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + } + } + + for (x = TEST_ATAN2_XY_STEP; x < M_PI_F; x += TEST_ATAN2_XY_STEP) + { + for (y = -TEST_ATAN2_XY_STEP; y > -M_PI_F; y -= TEST_ATAN2_XY_STEP) + { + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + } + } + + for (x = -TEST_ATAN2_XY_STEP; x > -M_PI_F; x -= TEST_ATAN2_XY_STEP) + { + for (y = TEST_ATAN2_XY_STEP; y < M_PI_F; y += TEST_ATAN2_XY_STEP) + { + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + } + } + + /* Test some big numbers */ + + x = 1000000.0; + y = 2.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + + x = 2.0; + y = 1000000.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + + x = 1000000.0; + y = 1000000.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + + x = -1000000.0; + y = 1000000.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + + x = 1000000.0; + y = -1000000.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); + + x = -1000000.0; + y = -1000000.0; + + angle_ref = atan2f(y, x); + angle = fast_atan2(y, x); + + TEST_ASSERT_FLOAT_WITHIN(TEST_ATAN2_DELTA, angle_ref, angle); +} + +/* Test angle normalization */ + +static void test_angle_norm(void) +{ + float angle = 0.0; + float per = 0.0; + float bottom = 0.0; + float top = 0.0; + + /* range = (0.0, 2PI) */ + + per = 2 * M_PI_F; + bottom = 0.0; + top = 2 * M_PI_F; + + /* in range */ + + angle = 0.0; + angle_norm(&angle, per, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.0, angle); + + /* in range */ + + angle = 1.0; + angle_norm(&angle, per, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(1.0, angle); + + /* wrap to 0.2 */ + + angle = 2 * M_PI_F + 0.2; + angle_norm(&angle, per, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.2, angle); + + /* wrap to 0.2 */ + + angle = -2 * M_PI_F + 0.2; + angle_norm(&angle, per, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.2, angle); +} + +/* Test angle normalization with 2*PI period length */ + +static void test_angle_norm_2pi(void) +{ + float angle = 0.0; + float bottom = 0.0; + float top = 0.0; + + /* range = (0.0, 2PI) */ + + bottom = 0.0; + top = 2 * M_PI_F; + + /* in range */ + + angle = 0.0; + angle_norm_2pi(&angle, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.0, angle); + + /* in range */ + + angle = 1.0; + angle_norm_2pi(&angle, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(1.0, angle); + + /* wrap to 0.2 */ + + angle = 2 * M_PI_F + 0.2; + angle_norm_2pi(&angle, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.2, angle); + + /* wrap to 0.2 */ + + angle = -2 * M_PI_F + 0.2; + angle_norm_2pi(&angle, bottom, top); + + TEST_ASSERT_EQUAL_FLOAT(0.2, angle); +} + +/* Test phase angle update */ + +static void test_phase_angle_update(void) +{ + struct phase_angle_s angle; + float val = 0.0; + float s = 0.0; + float c = 0.0; + + /* angle = 0.0 */ + + val = 0.0; + s = ANGLE_SIN(val); + c = ANGLE_COS(val); + phase_angle_update(&angle, val); + + TEST_ASSERT_EQUAL_FLOAT(val, angle.angle); + TEST_ASSERT_EQUAL_FLOAT(s, angle.sin); + TEST_ASSERT_EQUAL_FLOAT(c, angle.cos); + + /* angle = 1.5 */ + + val = 1.5; + s = ANGLE_SIN(val); + c = ANGLE_COS(val); + phase_angle_update(&angle, val); + + TEST_ASSERT_EQUAL_FLOAT(val, angle.angle); + TEST_ASSERT_EQUAL_FLOAT(s, angle.sin); + TEST_ASSERT_EQUAL_FLOAT(c, angle.cos); + + /* angle = 8, should be normalize to (0.0, 2PI) range */ + + val = 8; + s = ANGLE_SIN(val); + c = ANGLE_COS(val); + phase_angle_update(&angle, val); + + TEST_ASSERT_EQUAL_FLOAT(val-2*M_PI_F, angle.angle); + TEST_ASSERT_EQUAL_FLOAT(s, angle.sin); + TEST_ASSERT_EQUAL_FLOAT(c, angle.cos); + + /* angle = -1.5, should be normalize to (0.0, 2PI) range */ + + val = -1.5; + s = ANGLE_SIN(val); + c = ANGLE_COS(val); + phase_angle_update(&angle, val); + + TEST_ASSERT_EQUAL_FLOAT(val+2*M_PI_F, angle.angle); + TEST_ASSERT_EQUAL_FLOAT(s, angle.sin); + TEST_ASSERT_EQUAL_FLOAT(c, angle.cos); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_misc + ****************************************************************************/ + +void test_misc(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test helper funcions */ + + RUN_TEST(test_f_saturate); + + /* Test vector functions */ + + RUN_TEST(test_vector2d_mag); + RUN_TEST(test_vector2d_saturate); + RUN_TEST(test_dq_mag); + RUN_TEST(test_dq_saturate); + + /* Test fast trigonometric functions */ + + RUN_TEST(test_fast_sin); + RUN_TEST(test_fast_cos); + RUN_TEST(test_fast_sin2); + RUN_TEST(test_fast_cos2); + RUN_TEST(test_fast_atan2); + + /* Test angle functions */ + + RUN_TEST(test_angle_norm); + RUN_TEST(test_angle_norm_2pi); + RUN_TEST(test_phase_angle_update); + + UNITY_END(); +} diff --git a/examples/dsptest/test_motor.c b/examples/dsptest/test_motor.c new file mode 100644 index 000000000..89b24a64b --- /dev/null +++ b/examples/dsptest/test_motor.c @@ -0,0 +1,1141 @@ +/**************************************************************************** + * examples/dsptest/test_motor.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +/* Set float precision for this module */ + +#undef UNITY_FLOAT_PRECISION +#define UNITY_FLOAT_PRECISION (0.0001f) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Initialize openloop */ + +static void test_openloop_init(void) +{ + struct openloop_data_s op; + float angle = 0.0; + float max_speed = 100; + float per = 10e-6; + + /* Initialize openlooop controler */ + + motor_openloop_init(&op, max_speed, per); + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* Test values after initialization */ + + TEST_ASSERT_EQUAL_FLOAT(0.0, angle); + TEST_ASSERT_EQUAL_FLOAT(per, op.per); + TEST_ASSERT_EQUAL_FLOAT(max_speed, op.max); +} + +/* Singe step openloop */ + +static void test_openloop_one_step(void) +{ + struct openloop_data_s op; + float expected = 0.0; + float angle = 0.0; + float max_speed = 100; + float speed = 10; + float per = 10e-6; + + /* Initialize openlooop controler */ + + motor_openloop_init(&op, max_speed, per); + + /* Do single iteration in CW direction */ + + motor_openloop(&op, speed, DIR_CW); + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* Get expected value */ + + expected = speed * per; + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected, angle); + + /* Do single iteration in CCW direction */ + + motor_openloop(&op, speed, DIR_CCW); + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* Get expected value */ + + expected = 0.0; + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected, angle); +} + +/* Many steps in openloop */ + +static void test_openloop_many_steps(void) +{ + struct openloop_data_s op; + float expected = 0.0; + float angle = 0.0; + float max_speed = 100; + float speed = 10; + float per = 50e-6; + int iter = 10; + int i = 0; + + /* Initialize openlooop controler */ + + motor_openloop_init(&op, max_speed, per); + + /* Do some iterations in CW direction */ + + for (i = 0; i < iter; i+=1) + { + motor_openloop(&op, speed, DIR_CW); + } + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* Get expected value */ + + expected = speed * per * iter; + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected, angle); + + /* Do some iterations in CCW direction */ + + for (i = 0; i < iter; i+=1) + { + motor_openloop(&op, speed, DIR_CCW); + } + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* We should return to 0 */ + + expected = 0.0; + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected, angle); +} + +/* Test maximum openloop speed */ + +static void test_openloop_max_speed(void) +{ + TEST_IGNORE_MESSAGE("not implemented"); +} + +/* Normalize angle in openloop */ + +static void test_openloop_normalize_angle(void) +{ + struct openloop_data_s op; + float expected = 0.0; + float angle = 0.0; + float max_speed = 100; + float speed = 10; + float per = 10e-6; + int iter = 1000; + int i = 0; + + /* Initialize openlooop controler */ + + motor_openloop_init(&op, max_speed, per); + + /* Do many iterations to exceed 2PI range */ + + for (i = 0; i < iter; i+=1) + { + motor_openloop(&op, speed, DIR_CW); + } + + /* Get openloop angle */ + + angle = motor_openloop_angle_get(&op); + + /* Get expected value */ + + expected = speed * per * iter; + + /* And normalize to <0.0, 2*PI> */ + + while (expected > 2*M_PI_F) + { + expected -= 2*M_PI_F; + } + + /* Test angle */ + + TEST_ASSERT_EQUAL_FLOAT(expected, angle); +} + +/* Initialize otor angle */ + +static void test_angle_init(void) +{ + struct motor_angle_s angle; + float angle_m = 0.0; + float angle_e = 0.0; + uint8_t p = 0; + + /* Initialize motor angle */ + + p = 32; + motor_angle_init(&angle, p); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test initial values */ + + TEST_ASSERT_EQUAL_FLOAT(0.0, angle_e); + TEST_ASSERT_EQUAL_FLOAT(0.0, angle_m); + TEST_ASSERT_EQUAL_UINT8(p, angle.p); + TEST_ASSERT_EQUAL_FLOAT((float)1.0/p, angle.one_by_p); + TEST_ASSERT_EQUAL_INT8(0, angle.i); +} + +/* Update electrical angle in CW direction */ + +static void test_angle_el_update_cw(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Update electrical angle with 0.0 */ + + angle_step = 0.0; + expected_e = 0.0; + expected_m = 0.0; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_e_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + + /* Update electrical angle with 0.1 */ + + angle_step = 0.1; + expected_e = 0.1; + expected_m = 0.1/p; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_e_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + + /* Update electrical angle with 2*PI + 0.2 in three steps. + * This should increase pole counter in angle structure by 1. + */ + + angle_step = 2 * M_PI_F + 0.2; + expected_e = 0.2; + expected_m = angle_step/p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_e_update(&angle, M_PI_F, DIR_CW); + motor_angle_e_update(&angle, 2*M_PI_F, DIR_CW); + motor_angle_e_update(&angle, 0.2, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(1, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); +} + +/* Update electrical angle in CCW direction */ + +static void test_angle_el_update_ccw(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Move angle 0.1 in CCW direction from 0.0. + * We start from 0.0 and move angle CCW by 0.1. + */ + + angle_step = MOTOR_ANGLE_E_MAX-0.1; + expected_e = angle_step; + expected_m = (p-1)*MOTOR_ANGLE_M_MAX/p + expected_e/p; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_e_update(&angle, angle_step, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(p-1, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + + /* Update electrical angle with 2PI+0.1 in CCW direction in three steps */ + + angle_step = (MOTOR_ANGLE_E_MAX + 0.1); + expected_e = MOTOR_ANGLE_E_MAX-0.1; + expected_m = (p-2)*MOTOR_ANGLE_M_MAX/p + expected_e/p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_e_update(&angle, MOTOR_ANGLE_E_MAX-M_PI_F, DIR_CCW); + motor_angle_e_update(&angle, MOTOR_ANGLE_E_MAX-2*M_PI_F, DIR_CCW); + motor_angle_e_update(&angle, MOTOR_ANGLE_E_MAX-0.1, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(p-2, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); +} + +/* Update electrical angle and overflow electrical angle in CW direction */ + +static void test_angle_el_update_cw_overflow(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + float a = 0.0; + int i = 0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Update electrical angle to acheive full mechanical roatation */ + + angle_step = 0.1; + expected_e = angle_step; + expected_m = angle_step/p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < p; i += 1) + { + for (a = 0.0; a <= MOTOR_ANGLE_E_MAX; a += angle_step) + { + motor_angle_e_update(&angle, a, DIR_CW); + } + } + + /* Test poles counter before final step */ + + TEST_ASSERT_EQUAL_INT8(p-1, angle.i); + + /* One more step after overflow mechanical angle */ + + a = angle_step; + motor_angle_e_update(&angle, a, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); +} + +/* Update electrical angle and overflow electrical angle in CCW direction */ + +static void test_angle_el_update_ccw_overflow(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + float a = 0.0; + int i = 0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Update electrical angle to acheive full mechanical roatation */ + + angle_step = 0.1; + expected_e = MOTOR_ANGLE_E_MAX - angle_step; + expected_m = MOTOR_ANGLE_M_MAX - angle_step/p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < p; i += 1) + { + for (a = MOTOR_ANGLE_E_MAX; a >= 0.0; a -= angle_step) + { + motor_angle_e_update(&angle, a, DIR_CCW); + } + } + + /* Test poles counter before final step */ + + TEST_ASSERT_EQUAL_INT8(0, angle.i); + + /* One more step after overflow mechanical angle */ + + a = MOTOR_ANGLE_E_MAX - 0.1; + motor_angle_e_update(&angle, a, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(7, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); +} + +/* Update electric angle and change direction */ + +static void test_angle_el_change_dir(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + int i = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + float a = 0.0; + + /* Initialize motor angle */ + + p = 7; + motor_angle_init(&angle, p); + + /* Move electrical angle with 4*(2PI) + 0.1. + * It give us pole counter = 4 + */ + + angle_step = 0.1; + expected_m = 4*MOTOR_ANGLE_M_MAX/p + angle_step/p; + expected_e = 0.1; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < 4; i += 1) + { + for (a = 0.0; a <= MOTOR_ANGLE_E_MAX; a += angle_step) + { + motor_angle_e_update(&angle, a, DIR_CW); + } + } + + /* Test poles counter before final step */ + + TEST_ASSERT_EQUAL_INT8(3, angle.i); + + /* And rest 0.1 */ + + motor_angle_e_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(4, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + + /* Now move angle backward 2*(2PI) + 0.1 */ + + angle_step = 0.1; + expected_m = 2*MOTOR_ANGLE_M_MAX/p - angle_step/p; + expected_e = MOTOR_ANGLE_E_MAX - angle_step; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < 2; i += 1) + { + for (a = angle_m; a >= 0.0; a -= angle_step) + { + motor_angle_e_update(&angle, a, DIR_CCW); + } + } + + /* Test poles counter before final step */ + + TEST_ASSERT_EQUAL_INT8(2, angle.i); + + /* And rest 0.1 */ + + motor_angle_e_update(&angle, MOTOR_ANGLE_E_MAX-angle_step, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(1, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + + /* and again in forward direction 4*(2PI) + 0.1 */ + + angle_step = 0.1; + expected_m = 5*MOTOR_ANGLE_M_MAX/p + angle_step/p; + expected_e = 0.1; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < 4; i += 1) + { + for (a = angle_e; a <= MOTOR_ANGLE_E_MAX; a += angle_step) + { + motor_angle_e_update(&angle, a, DIR_CW); + } + } + + /* Test poles counter before final step */ + + TEST_ASSERT_EQUAL_INT8(4, angle.i); + + /* And rest 0.1 */ + + motor_angle_e_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(5, angle.i); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); +} + +/* Update mechanical angle in CW direction */ + +static void test_angle_m_update_cw(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Update mechanical angle with 0.0 */ + + angle_step = 0.0; + expected_m = 0.0; + expected_e = 0.0; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_m_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); + + /* Update mechanical angle with 0.1 */ + + angle_step = 0.1; + expected_m = angle_step; + expected_e = angle_step*p - 0*MOTOR_ANGLE_E_MAX/p; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_m_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); + + /* Update mechanical angle to get one electrical angle rotation + 0.1 */ + + angle_step = MOTOR_ANGLE_M_MAX/p + 0.1; + expected_m = angle_step; + expected_e = angle_step*p - 1*MOTOR_ANGLE_E_MAX; + + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, angle_step/3, DIR_CW); + motor_angle_m_update(&angle, 2*angle_step/3, DIR_CW); + motor_angle_m_update(&angle, 3*angle_step/3, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(1, angle.i); +} + +/* Update mechanical angle in CCW direction */ + +static void test_angle_m_update_ccw(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 8; + motor_angle_init(&angle, p); + + /* Update mechanical angle with 1.0 + * For 8 poles, one electrical angle rotationa takes ~0.785. + * So with angle step = 1.0 we have 1 electical angle rotation plus some rest. + */ + + angle_step = 1.0; + expected_m = angle_step; + expected_e = angle_step*p - 1*MOTOR_ANGLE_E_MAX; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_m_update(&angle, angle_step, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(1, angle.i); + + /* Update mechanical angle to get one electrical angle rotation */ + + angle_step = angle_step - MOTOR_ANGLE_E_MAX/p; + expected_m = angle_step; + expected_e = angle_step*p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, angle_step/3, DIR_CCW); + motor_angle_m_update(&angle, 2*angle_step/3, DIR_CCW); + motor_angle_m_update(&angle, 3*angle_step/3, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); +} + +/* Update mechanical angle and overflow mechanical angle in CW direction */ + +static void test_angle_m_update_cw_overflow(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 3; + motor_angle_init(&angle, p); + + /* Full mechanical angle rotation (2PI) + 0.1 in CW direction */ + + angle_step = 0.1; + expected_m = angle_step; + expected_e = 0*MOTOR_ANGLE_E_MAX/p + angle_step*p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, 0.0, DIR_CW); + motor_angle_m_update(&angle, MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 1*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 2*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 3*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 4*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, angle_step, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(0, angle.i); +} + +/* Update mechanical angle and overflow mechanical angle in CCW direction */ + +static void test_angle_m_update_ccw_overflow(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 3; + motor_angle_init(&angle, p); + + /* Full mechanical angle rotation (2PI) + 0.1 in CCW direction */ + + angle_step = MOTOR_ANGLE_M_MAX - 0.1; + expected_m = angle_step; + expected_e = MOTOR_ANGLE_E_MAX - 0.1*p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, 0.0, DIR_CCW); + motor_angle_m_update(&angle, 4*MOTOR_ANGLE_M_MAX/4, DIR_CCW); + motor_angle_m_update(&angle, 3*MOTOR_ANGLE_M_MAX/4, DIR_CCW); + motor_angle_m_update(&angle, 2*MOTOR_ANGLE_M_MAX/4, DIR_CCW); + motor_angle_m_update(&angle, 1*MOTOR_ANGLE_M_MAX/4, DIR_CCW); + motor_angle_m_update(&angle, angle_step, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(p-1, angle.i); +} + +/* Update mechanical angle and change direction */ + +static void test_angle_m_change_dir(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float expected_i = 0.0; + float s = 0.0; + float c = 0.0; + + /* Initialize motor angle */ + + p = 3; + motor_angle_init(&angle, p); + + /* Move mechanical angle by 3*(2PI)/4 in CW direction */ + + angle_step = 3*MOTOR_ANGLE_M_MAX/4; + expected_m = angle_step; + expected_i = ((int)(angle_step*p/MOTOR_ANGLE_M_MAX)); + expected_e = angle_step*p - expected_i*MOTOR_ANGLE_E_MAX; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, 0.0, DIR_CW); + motor_angle_m_update(&angle, 1*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 2*MOTOR_ANGLE_M_MAX/4, DIR_CW); + motor_angle_m_update(&angle, 3*MOTOR_ANGLE_M_MAX/4, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(expected_i, angle.i); + + /* Move mechanical angle by 1.0 in CCW direction */ + + angle_step = 3*MOTOR_ANGLE_M_MAX/4 - 2.0; + expected_m = angle_step; + expected_i = ((int)(angle_step*p/MOTOR_ANGLE_M_MAX)); + expected_e = angle_step*p - expected_i*MOTOR_ANGLE_E_MAX; + s = sin(expected_e); + c = cos(expected_e); + + motor_angle_m_update(&angle, 3*MOTOR_ANGLE_M_MAX/4-1.0, DIR_CCW); + motor_angle_m_update(&angle, 3*MOTOR_ANGLE_M_MAX/4-2.0, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(expected_i, angle.i); +} + +/* Mix update mechanical angle and update electrical angle */ + +static void test_angle_m_el_mixed(void) +{ + struct motor_angle_s angle; + uint8_t p = 0; + int i = 0; + float angle_step = 0.0; + float angle_m = 0.0; + float angle_e = 0.0; + float expected_e = 0.0; + float expected_m = 0.0; + float expected_i = 0.0; + float s = 0.0; + float c = 0.0; + float a = 0.0; + + /* Initialize motor angle */ + + p = 27; + motor_angle_init(&angle, p); + + /* Update mechanical angle to get 4 electrical angle + * rotations + 0.1 in CW direction + */ + + angle_step = 4*MOTOR_ANGLE_M_MAX/p + 0.1; + expected_m = angle_step; + expected_i = ((int)(angle_step*p/MOTOR_ANGLE_M_MAX)); + expected_e = angle_step*p - expected_i*MOTOR_ANGLE_E_MAX; + s = sin(expected_e); + c = cos(expected_e); + + /* Move in a few steps */ + + motor_angle_m_update(&angle, 0.0, DIR_CW); + motor_angle_m_update(&angle, MOTOR_ANGLE_M_MAX/p, DIR_CW); + motor_angle_m_update(&angle, 4*MOTOR_ANGLE_M_MAX/p, DIR_CW); + motor_angle_m_update(&angle, 4*MOTOR_ANGLE_M_MAX/p + 0.1, DIR_CW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(expected_i, angle.i); + + /* Now move electrical angle by 2 full rotation in CCW direction. + * This should give us the same electrical angle and mechanical + * angle reduced by 2 electrical rotations. + */ + + angle_step = 2*MOTOR_ANGLE_E_MAX; + expected_i = expected_i - 2; + expected_e = expected_e; + expected_m = expected_m - 2*MOTOR_ANGLE_M_MAX/p; + s = sin(expected_e); + c = cos(expected_e); + + /* Move angle in loop */ + + for (i = 0; i < 2; i+=1) + { + for (a = expected_e ; a >= 0.0; a -= 0.1) + { + motor_angle_e_update(&angle, a, DIR_CCW); + } + } + + /* Final step */ + + motor_angle_e_update(&angle, expected_e, DIR_CCW); + + angle_m = motor_angle_m_get(&angle); + angle_e = motor_angle_e_get(&angle); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(expected_e, angle_e); + TEST_ASSERT_EQUAL_FLOAT(expected_m, angle_m); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, s, angle.angle_el.sin); + TEST_ASSERT_FLOAT_WITHIN(TEST_SINCOS_DELTA, c, angle.angle_el.cos); + TEST_ASSERT_EQUAL_INT8(expected_i, angle.i); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_motor + ****************************************************************************/ + +void test_motor(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test some definitions */ + + TEST_ASSERT_EQUAL_FLOAT(1.0, DIR_CW); + TEST_ASSERT_EQUAL_FLOAT(-1.0, DIR_CCW); + + /* Openloop control functions */ + + RUN_TEST(test_openloop_init); + RUN_TEST(test_openloop_one_step); + RUN_TEST(test_openloop_many_steps); + RUN_TEST(test_openloop_max_speed); + RUN_TEST(test_openloop_normalize_angle); + + /* Motor angle */ + + RUN_TEST(test_angle_init); + RUN_TEST(test_angle_el_update_cw); + RUN_TEST(test_angle_el_update_ccw); + RUN_TEST(test_angle_el_update_cw_overflow); + RUN_TEST(test_angle_el_update_ccw_overflow); + RUN_TEST(test_angle_el_change_dir); + RUN_TEST(test_angle_m_update_cw); + RUN_TEST(test_angle_m_update_ccw); + RUN_TEST(test_angle_m_update_cw_overflow); + RUN_TEST(test_angle_m_update_ccw_overflow); + RUN_TEST(test_angle_m_change_dir); + RUN_TEST(test_angle_m_el_mixed); + + UNITY_END(); +} diff --git a/examples/dsptest/test_observer.c b/examples/dsptest/test_observer.c new file mode 100644 index 000000000..bfbd91c76 --- /dev/null +++ b/examples/dsptest/test_observer.c @@ -0,0 +1,350 @@ +/**************************************************************************** + * examples/dsptest/test_observer.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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(); +} diff --git a/examples/dsptest/test_pid.c b/examples/dsptest/test_pid.c new file mode 100644 index 000000000..e67f64caf --- /dev/null +++ b/examples/dsptest/test_pid.c @@ -0,0 +1,516 @@ +/**************************************************************************** + * examples/dsptest/test_pid.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +#define TEST_NAME "PID" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Initialize PI controller */ + +static void test_pi_controller_init(void) +{ + pid_controller_t pi; + float max = 0.990; + float min = 0.001; + float kp = 2.0; + float ki = 0.001; + + /* Initialize PI controller */ + + pi_controller_init(&pi, kp, ki); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(kp, pi.KP); + TEST_ASSERT_EQUAL_FLOAT(ki, pi.KI); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.KD); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.out); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.err); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.err_prev); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.part[0]); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.part[1]); + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.part[2]); + + /* Initialize saturation */ + + pi_saturation_set(&pi, min, max); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(max, pi.sat.max); + TEST_ASSERT_EQUAL_FLOAT(min, pi.sat.min); +} + +/* Feed PI controller with zeros */ + +static void test_pi_controller_zeros(void) +{ + pid_controller_t pi; + float max = 0.990; + float min = 0.000; + float kp = 2.0; + float ki = 0.001; + int i = 0; + + /* Initialize PI controller */ + + pi_controller_init(&pi, kp, ki); + + /* Initialize saturation */ + + pid_saturation_set(&pi, min, max); + + for (i=0; i< 10000; i+=1) + { + pi_controller(&pi, 0.0); + } + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(0.0, pi.out); +} + +/* Feed PI controller with some data */ + +static void test_pi_controller(void) +{ + pid_controller_t pi; + float kp = 0.0; + float ki = 0.0; + float max = 0.0; + float min = 0.0; + float p1 = 0.0; + float p2 = 0.0; + float p3 = 0.0; + float out = 0.0; + float err = 0.0; + int i = 0; + + kp = 1.0; + ki = 0.1; + pi_controller_init(&pi, kp, ki); + + /* Step 1 */ + + err = 0.1; + + pi_controller(&pi, err); + + p1 = err * kp; + p2 += err * ki; + p3 = err * 0; + out = p1 + p2 + p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pi.out); + + /* Step 2 */ + + err = 0.2; + + pi_controller(&pi, err); + p1 = err * kp; + p2 += err * ki; + p3 = err * 0; + out = p1 + p2 + p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pi.out); + + /* Step 3 */ + + err = -0.3; + + pi_controller(&pi, err); + p1 = err * kp; + p2 += err * ki; + p3 = err * 0; + out = p1 + p2 + p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pi.out); + + /* Now set saturation */ + + max = 0.3; + min = 0.0; + pi_saturation_set(&pi, min, max); + + /* Test saturation max */ + + err = 0.4; + for (i = 0; i < 20; i += 1) + { + pi_controller(&pi, err); + } + + out = max; + TEST_ASSERT_EQUAL_FLOAT(out, pi.out); + + err = -0.8; + for (i = 0; i < 20; i += 1) + { + pi_controller(&pi, err); + } + + /* Test saturation min */ + + out = min; + TEST_ASSERT_EQUAL_FLOAT(out, pi.out); +} + +/* Saturate PI controller */ + +static void test_pi_controller_saturation(void) +{ + pid_controller_t pi; + float max = 0.990; + float min = 0.000; + float kp = 2.0; + float ki = 0.001; + int i = 0; + + /* Initialize PI controller */ + + pi_controller_init(&pi, kp, ki); + + /* Initialize saturation */ + + pi_saturation_set(&pi, min, max); + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pi_controller(&pi, 0.01); + TEST_ASSERT_LESS_OR_EQUAL(max, pi.out); + TEST_ASSERT_LESS_OR_EQUAL(max, pi.part[1]); + } + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pi_controller(&pi, -0.01); + TEST_ASSERT_GREATER_OR_EQUAL(min, pi.out); + TEST_ASSERT_GREATER_OR_EQUAL(min, pi.part[1]); + } +} + +/* PI windup protection */ + +static void test_pi_windup_protection(void) +{ + pid_controller_t pi; + float max = 0.990; + float min = 0.000; + float kp = 2.0; + float ki = 0.1; + int i = 0; + + /* Initialize PI controller */ + + pi_controller_init(&pi, kp, ki); + + /* Initialize saturation */ + + pi_saturation_set(&pi, min, max); + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pi_controller(&pi, 0.01); + TEST_ASSERT_LESS_OR_EQUAL(max, pi.part[1]); + } + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pi_controller(&pi, -0.01); + TEST_ASSERT_GREATER_OR_EQUAL(min, pi.part[1]); + } +} + +/* Initialize PID controller */ + +static void test_pid_controller_init(void) +{ + pid_controller_t pid; + float max = 0.990; + float min = 0.001; + float kp = 2.0; + float ki = 0.001; + float kd = 0.001; + + /* Initialize PID controller */ + + pid_controller_init(&pid, kp, ki, kd); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(kp, pid.KP); + TEST_ASSERT_EQUAL_FLOAT(ki, pid.KI); + TEST_ASSERT_EQUAL_FLOAT(kd, pid.KD); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.out); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.err); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.err_prev); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.part[0]); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.part[1]); + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.part[2]); + + /* Initialize saturation */ + + pid_saturation_set(&pid, min, max); + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(max, pid.sat.max); + TEST_ASSERT_EQUAL_FLOAT(min, pid.sat.min); +} + +/* Feed PID controller with zeros */ + +static void test_pid_controller_zeros(void) +{ + pid_controller_t pid; + float max = 0.990; + float min = 0.000; + float kp = 2.0; + float ki = 0.001; + float kd = 0.001; + int i = 0; + + /* Initialize PI controller */ + + pid_controller_init(&pid, kp, ki, kd); + + /* Initialize saturation */ + + pid_saturation_set(&pid, min, max); + + for (i=0; i< 10000; i+=1) + { + pid_controller(&pid, 0.0); + } + + /* Test */ + + TEST_ASSERT_EQUAL_FLOAT(0.0, pid.out); +} + +/* Feed PID controller with some data */ + +static void test_pid_controller(void) +{ + pid_controller_t pid; + float kp = 0.0; + float ki = 0.0; + float kd = 0.0; + float max = 0.0; + float min = 0.0; + float p1 = 0.0; + float p2 = 0.0; + float p3 = 0.0; + float out = 0.0; + float err = 0.0; + float prev = 0.0; + int i = 0; + + kp = 1.0; + ki = 0.1; + kd = 0.01; + pid_controller_init(&pid, kp, ki, kd); + + /* Step 1 */ + + prev = 0.0; + err = 0.1; + + pid_controller(&pid, err); + + p1 = err * kp; + p2 += err * ki; + p3 = (err - prev) * kd; + out = p1 + p2 + p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pid.out); + + /* Step 2 */ + + prev = err; + err = 0.2; + + pid_controller(&pid, err); + p1 = err * kp; + p2 += err * ki; + p3 = (err - prev) * kd; + out = p1 + p2 + p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pid.out); + + /* Step 3 */ + + prev = err; + err = -0.3; + + pid_controller(&pid, err); + p1 = err * kp; + p2 += err * ki; + p3 = (err - prev) * kd; + out = p1 + p2 +p3; + + TEST_ASSERT_EQUAL_FLOAT(out, pid.out); + + /* Now set saturation */ + + max = 0.3; + min = 0.0; + pid_saturation_set(&pid, min, max); + + /* Test saturation max */ + + prev = err; + err = 0.4; + for (i = 0; i < 20; i += 1) + { + pid_controller(&pid, err); + } + + out = max; + TEST_ASSERT_EQUAL_FLOAT(out, pid.out); + + prev = err; + err = -0.8; + for (i = 0; i < 20; i += 1) + { + pid_controller(&pid, err); + } + + /* Test saturation min */ + + out = min; + TEST_ASSERT_EQUAL_FLOAT(out, pid.out); +} + +/* Saturate PID controller */ + +static void test_pid_controller_saturation(void) +{ + pid_controller_t pid; + float max = 0.990; + float min = 0.000; + float kp = 2.0; + float ki = 0.001; + float kd = 0.001; + int i = 0; + + /* Initialize PID controller */ + + pid_controller_init(&pid, kp, ki, kd); + + /* Initialize saturation */ + + pid_saturation_set(&pid, min, max); + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pid_controller(&pid, 0.01); + TEST_ASSERT_LESS_OR_EQUAL(max, pid.out); + TEST_ASSERT_LESS_OR_EQUAL(max, pid.part[1]); + } + + /* Feed controller */ + + for (i=0; i< 1000; i+=1) + { + pid_controller(&pid, -0.01); + TEST_ASSERT_GREATER_OR_EQUAL(min, pid.out); + TEST_ASSERT_GREATER_OR_EQUAL(min, pid.part[1]); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_pid + ****************************************************************************/ + +void test_pid(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test PID functions */ + + RUN_TEST(test_pi_controller_init); + RUN_TEST(test_pi_controller_zeros); + RUN_TEST(test_pi_controller); + RUN_TEST(test_pi_controller_saturation); + RUN_TEST(test_pi_windup_protection); + RUN_TEST(test_pid_controller_init); + RUN_TEST(test_pid_controller_zeros); + RUN_TEST(test_pid_controller); + RUN_TEST(test_pid_controller_saturation); + + UNITY_END(); +} diff --git a/examples/dsptest/test_svm.c b/examples/dsptest/test_svm.c new file mode 100644 index 000000000..d96660f5b --- /dev/null +++ b/examples/dsptest/test_svm.c @@ -0,0 +1,310 @@ +/**************************************************************************** + * examples/dsptest/test_svm.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +#define SVM3_DUTY_MAX 0.95 +#define SVM3_DUTY_MIN 0.00 + +#undef UNITY_FLOAT_PRECISION +#define UNITY_FLOAT_PRECISION (0.0001f) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void SVM3(FAR struct svm3_state_s *s, float a, float b) +{ + ab_frame_t ab; + + svm3_init(s, SVM3_DUTY_MIN, SVM3_DUTY_MAX); + + ab.a = a; + ab.b = b; + + svm3(s, &ab); +} + +/* Feed SVM3 with zeros */ + +static void test_svm3_zero(void) +{ + struct svm3_state_s s; + + SVM3(&s, 0.0, 0.0); + + TEST_ASSERT_EQUAL_FLOAT(0.5, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.5, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.5, s.d_w); + TEST_ASSERT_EQUAL_INT8(2, s.sector); +} + +/* Test duty cycle saturation */ + +static void test_svm3_saturation(void) +{ + struct svm3_state_s s; + + /* v_a = 1.0 + * v_b = 1.0 + * mag(v_ab) > 1.0 which is not valid for SVM + * d_u > 1.0, d_w < 0.0 + */ + + SVM3(&s, 1.0, 1.0); + + TEST_ASSERT_EQUAL_FLOAT(SVM3_DUTY_MAX, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.81698, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(SVM3_DUTY_MIN, s.d_w); + TEST_ASSERT_EQUAL_INT8(1, s.sector); + + + /* v_a = -1.0 + * v_b = -1.0 + * mag(v_ab) > 1.0 which is not valid for SVM + * d_u < 0.0, dw > 1 + */ + + SVM3(&s, -1.0, -1.0); + + TEST_ASSERT_EQUAL_FLOAT(SVM3_DUTY_MIN, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.183012, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(SVM3_DUTY_MAX, s.d_w); + TEST_ASSERT_EQUAL_INT8(4, s.sector); +} + +/* Test vectors from sector 1 to sector 6 */ + +static void test_svm3_s1s6(void) +{ + struct svm3_state_s s; + + /* Vector in sector 1 */ + + SVM3(&s, 0.5, 0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.84150, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.65849, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.15849, s.d_w); + TEST_ASSERT_EQUAL_INT8(1, s.sector); + + /* Vector in sector 2 */ + + SVM3(&s, 0.0, 0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.5, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.75, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.25, s.d_w); + TEST_ASSERT_EQUAL_INT8(2, s.sector); + + /* Vector in sector 3 */ + + SVM3(&s, -0.5, 0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.15849, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.84150, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.34150, s.d_w); + TEST_ASSERT_EQUAL_INT8(3, s.sector); + + /* Vector in sector 4 */ + + SVM3(&s, -0.5, -0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.15849, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.34150, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.84150, s.d_w); + TEST_ASSERT_EQUAL_INT8(4, s.sector); + + /* Vector in sector 5 */ + + SVM3(&s, 0.0, -0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.5, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.25, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.75, s.d_w); + TEST_ASSERT_EQUAL_INT8(5, s.sector); + + /* Vector in sector 6 */ + + SVM3(&s, 0.5, -0.5); + + TEST_ASSERT_EQUAL_FLOAT(0.84150, s.d_u); + TEST_ASSERT_EQUAL_FLOAT(0.15849, s.d_v); + TEST_ASSERT_EQUAL_FLOAT(0.65849, s.d_w); + TEST_ASSERT_EQUAL_INT8(6, s.sector); +} + +/* Get SVM3 base voltage */ + +static void test_svm3_vbase(void) +{ + float vbus = 0.0; + float vbase = 0.0; + + vbus = 10.0; + vbase = SVM3_BASE_VOLTAGE_GET(vbus); + TEST_ASSERT_EQUAL_FLOAT(5.77350, vbase); + + vbus = 78.0; + vbase = SVM3_BASE_VOLTAGE_GET(vbus); + TEST_ASSERT_EQUAL_FLOAT(45.0333, vbase); +} + +/* Correct ADC samples according to SVM3 state */ + +static void test_svm3_adc_correct(void) +{ + struct svm3_state_s s; + int32_t c1 = 0; + int32_t c2 = 0; + int32_t c3 = 0; + + /* Sector 1 - ignore phase 1 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, 0.5, 0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(20, c1); + TEST_ASSERT_EQUAL_INT(10, c2); + TEST_ASSERT_EQUAL_INT(-30, c3); + + /* Sector 2 - ignore phase 2 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, 0.0, 0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(100, c1); + TEST_ASSERT_EQUAL_INT(-70, c2); + TEST_ASSERT_EQUAL_INT(-30, c3); + + /* Sector 3 - ignore phase 2 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, -0.5, 0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(100, c1); + TEST_ASSERT_EQUAL_INT(-70, c2); + TEST_ASSERT_EQUAL_INT(-30, c3); + + /* Sector 4 - ignore phase 3 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, -0.5, -0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(100, c1); + TEST_ASSERT_EQUAL_INT(10, c2); + TEST_ASSERT_EQUAL_INT(-110, c3); + + /* Sector 5 - ignore phase 3 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, 0.0, -0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(100, c1); + TEST_ASSERT_EQUAL_INT(10, c2); + TEST_ASSERT_EQUAL_INT(-110, c3); + + /* Sector 6 - ignore phase 1 */ + + c1 = 100; + c2 = 10; + c3 = -30; + SVM3(&s, 0.5, -0.5); + svm3_current_correct(&s, &c1, &c2, &c3); + + TEST_ASSERT_EQUAL_INT(20, c1); + TEST_ASSERT_EQUAL_INT(10, c2); + TEST_ASSERT_EQUAL_INT(-30, c3); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_svm + ****************************************************************************/ + +void test_svm(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test 3 phase space vector modulation */ + + RUN_TEST(test_svm3_zero); + RUN_TEST(test_svm3_saturation); + RUN_TEST(test_svm3_s1s6); + RUN_TEST(test_svm3_vbase); + RUN_TEST(test_svm3_adc_correct); + + UNITY_END(); +} diff --git a/examples/dsptest/test_transform.c b/examples/dsptest/test_transform.c new file mode 100644 index 000000000..d315d7a1a --- /dev/null +++ b/examples/dsptest/test_transform.c @@ -0,0 +1,303 @@ +/**************************************************************************** + * examples/dsptest/test_transform.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * 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 + ****************************************************************************/ + +/* Set float precision for this module */ + +#undef UNITY_FLOAT_PRECISION +#define UNITY_FLOAT_PRECISION (0.0001f) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Feed Clarke transform with some data */ + +static void test_transform_clarke(void) +{ + abc_frame_t abc; + ab_frame_t ab; + + /* a = 0.0, b = 0.0, c = 0.0 -> alpha = 0.0, beta = 0.0 */ + + abc.a = 0.0; + abc.b = 0.0; + abc.c = 0.0; + clarke_transform(&abc, &ab); + + TEST_ASSERT_EQUAL_FLOAT(0.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(0.0, ab.b); + + /* a = 1.0, b = -2.0, c = 1.0 -> alpha = 1.0, beta = -1.732 */ + + abc.a = 1.0; + abc.b = -2.0; + abc.c = 1.0; + clarke_transform(&abc, &ab); + + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(-1.7320, ab.b); + + /* a = 1.0, b = 1.0, c = -2.0 -> alpha = 1.0, beta = 1.7320 */ + + abc.a = 1.0; + abc.b = 1.0; + abc.c = -2.0; + clarke_transform(&abc, &ab); + + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.7320, ab.b); + + /* a = -2.0, b = 1.0, c = 1.0 -> alpha = -2.0, beta = 0.0 */ + + abc.a = -2.0; + abc.b = 1.0; + abc.c = 1.0; + clarke_transform(&abc, &ab); + + TEST_ASSERT_EQUAL_FLOAT(-2.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(0.0, ab.b); +} + +/* Feed inverse Clarke transform with some data */ + +static void test_transform_invclarke(void) +{ + ab_frame_t ab; + abc_frame_t abc; + + /* alpha = 0.0, beta = 0.0 -> a = 0.0, b = 0.0, c = 0.0 */ + + ab.a = 0.0; + ab.b = 0.0; + inv_clarke_transform(&ab, &abc); + + TEST_ASSERT_EQUAL_FLOAT(0.0, abc.a); + TEST_ASSERT_EQUAL_FLOAT(0.0, abc.b); + TEST_ASSERT_EQUAL_FLOAT(0.0, abc.c); + + /* alpha = 1.0, beta = 1.0 -> a = 1.0, b = 0.3660, c = -1.3660 */ + + ab.a = 1.0; + ab.b = 1.0; + inv_clarke_transform(&ab, &abc); + + TEST_ASSERT_EQUAL_FLOAT(1.0, abc.a); + TEST_ASSERT_EQUAL_FLOAT(0.3660, abc.b); + TEST_ASSERT_EQUAL_FLOAT(-1.3660, abc.c); + + /* alpha = -1.0, beta = -1.0 -> a = -1.0, b = -0.3660, c = 1.3660 */ + + ab.a = -1.0; + ab.b = -1.0; + inv_clarke_transform(&ab, &abc); + + TEST_ASSERT_EQUAL_FLOAT(-1.0, abc.a); + TEST_ASSERT_EQUAL_FLOAT(-0.3660, abc.b); + TEST_ASSERT_EQUAL_FLOAT(1.3660, abc.c); + + /* alpha = 1.0, beta = -1.0 -> a = 1.0, b = -1.3660, c = 0.3660 */ + + ab.a = 1.0; + ab.b = -1.0; + inv_clarke_transform(&ab, &abc); + + TEST_ASSERT_EQUAL_FLOAT(1.0, abc.a); + TEST_ASSERT_EQUAL_FLOAT(-1.3660, abc.b); + TEST_ASSERT_EQUAL_FLOAT(0.3660, abc.c); +} + +/* Feed Park transform with some data */ + +static void test_transform_park(void) +{ + phase_angle_t angle; + ab_frame_t ab; + dq_frame_t dq; + + /* angle = 0.0, alpha = 0.0, beta = 0.0 -> d = 0.0, q = 0.0 */ + + phase_angle_update(&angle, 0.0); + ab.a = 0.0; + ab.b = 0.0; + park_transform(&angle, &ab, &dq); + + TEST_ASSERT_EQUAL_FLOAT(0.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(0.0, dq.q); + + /* angle = 0.0, alpha = 1.0, beta = 1.0 -> d = 1.0, q = 1.0 */ + + phase_angle_update(&angle, 0.0); + ab.a = 1.0; + ab.b = 1.0; + park_transform(&angle, &ab, &dq); + + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.q); + + /* angle = PI, alpha = 1.0, beta = 1.0 -> d = -1.0, q = -1.0 */ + + phase_angle_update(&angle, M_PI_F); + ab.a = 1.0; + ab.b = 1.0; + park_transform(&angle, &ab, &dq); + + TEST_ASSERT_EQUAL_FLOAT(-1.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(-1.0, dq.q); + + /* angle = PI, alpha = -1.0, beta = 1.0 -> d = 1.0, q = -1.0 */ + + phase_angle_update(&angle, M_PI_F); + ab.a = -1.0; + ab.b = 1.0; + park_transform(&angle, &ab, &dq); + + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(-1.0, dq.q); + + /* angle = -PI/2, alpha = 1.0, beta = 1.0 -> d = -1.0, q = 1.0 */ + + phase_angle_update(&angle, -M_PI_F/2); + ab.a = 1.0; + ab.b = 1.0; + park_transform(&angle, &ab, &dq); + + TEST_ASSERT_EQUAL_FLOAT(-1.0, dq.d); + TEST_ASSERT_EQUAL_FLOAT(1.0, dq.q); +} + +/* Feed inverse Park transform with some data */ + +static void test_transform_invpark(void) +{ + phase_angle_t angle; + dq_frame_t dq; + ab_frame_t ab; + + /* angle = 0.0, d = 0.0, q = 0.0 -> alpha = 0.0, beta = 0.0 */ + + phase_angle_update(&angle, 0.0); + dq.d = 0.0; + dq.q = 0.0; + inv_park_transform(&angle, &dq, &ab); + + TEST_ASSERT_EQUAL_FLOAT(0.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(0.0, ab.b); + + /* angle = 0.0, d = 1.0, q = 1.0 -> alpha = 1.0, beta = 1.0 */ + + phase_angle_update(&angle, 0.0); + dq.d = 1.0; + dq.q = 1.0; + inv_park_transform(&angle, &dq, &ab); + + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.b); + + /* angle = PI, d = 1.0, q = 1.0 -> alpha = 0.0, beta = 0.0 */ + + phase_angle_update(&angle, M_PI_F); + dq.d = 1.0; + dq.q = 1.0; + inv_park_transform(&angle, &dq, &ab); + + TEST_ASSERT_EQUAL_FLOAT(-1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(-1.0, ab.b); + + /* angle = PI, d = -1.0, q = 1.0 -> alpha = 0.0, beta = 0.0 */ + + phase_angle_update(&angle, M_PI_F); + dq.d = -1.0; + dq.q = 1.0; + inv_park_transform(&angle, &dq, &ab); + + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(-1.0, ab.b); + + /* angle = -PI/2, d = 1.0, q = 1.0 -> alpha = 0.0, beta = 0.0 */ + + phase_angle_update(&angle, -M_PI_F/2); + dq.d = 1.0; + dq.q = 1.0; + inv_park_transform(&angle, &dq, &ab); + + TEST_ASSERT_EQUAL_FLOAT(1.0, ab.a); + TEST_ASSERT_EQUAL_FLOAT(-1.0, ab.b); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_transform + ****************************************************************************/ + +void test_transform(void) +{ + UNITY_BEGIN(); + + TEST_SEPARATOR(); + + /* Test 3 phase motor transformations */ + + RUN_TEST(test_transform_clarke); + RUN_TEST(test_transform_invclarke); + RUN_TEST(test_transform_park); + RUN_TEST(test_transform_invpark); + + UNITY_END(); +}