sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merged in raiden00/apps (pull request #143)

Browse Source 
initial commit for examples/dsptest - unit test for Nuttx DSP library

Approved-by: GregoryN <gnutt@nuttx.org>
This commit is contained in:
Mateusz Szafoni 2018-07-07 17:09:43 +00:00 committed by GregoryN
parent db77b1ec6c
commit e66470df68
14 changed files with 4127 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
examples/README.txt
View File

@ -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
^^^^^^^^^^^^

11
examples/dsptest/.gitignore vendored Normal file
View File

@ -0,0 +1,11 @@
/Make.dep
/.depend
/.built
/*.asm
/*.obj
/*.rel
/*.lst
/*.sym
/*.adb
/*.lib
/*.src

30
examples/dsptest/Kconfig Normal file
View File

@ -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

39
examples/dsptest/Make.defs Normal file
View File

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

59
examples/dsptest/Makefile Normal file
View File

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

109
examples/dsptest/dsptest.h Normal file
View File

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

117
examples/dsptest/dsptest_main.c Normal file
View File

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

493
examples/dsptest/test_foc.c Normal file
View File

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

633
examples/dsptest/test_misc.c Normal file
View File

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

1141
examples/dsptest/test_motor.c Normal file
View File

File diff suppressed because it is too large Load Diff

350
examples/dsptest/test_observer.c Normal file
View File

@ -0,0 +1,350 @@
/****************************************************************************
* examples/dsptest/test_observer.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include "dsptest.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/* Dummy angle observer */
struct motor_observer_dummy_s
{
float x;
};
/* Dummy speed observer */
struct motor_sobserver_dummy_s
{
float x;
};
/****************************************************************************
* Private Function Protototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/* Initialize observer data */
static void test_observer_init(void)
{
struct motor_observer_dummy_s ao;
struct motor_sobserver_dummy_s so;
struct motor_observer_s o;
float per = 1e-11;
motor_observer_init(&o, (void*)&ao, (void*)&so, per);
TEST_ASSERT_EQUAL_FLOAT(0.0, o.angle);
TEST_ASSERT_EQUAL_FLOAT(0.0, o.speed);
TEST_ASSERT_EQUAL_FLOAT(per, o.per);
TEST_ASSERT_NOT_NULL(o.ao);
TEST_ASSERT_NOT_NULL(o.so);
}
/* Initialize SMO observer data */
static void test_observer_smo_init(void)
{
struct motor_observer_smo_s ao;
float kslide = 0.99;
float err_max = 0.99;
/* Initialize SMO observer */
motor_observer_smo_init(&ao, kslide, err_max);
/* Test */
TEST_ASSERT_EQUAL_FLOAT(kslide, ao.k_slide);
TEST_ASSERT_EQUAL_FLOAT(err_max, ao.err_max);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.F_gain);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.G_gain);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf_lp_filter1);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf_lp_filter2);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.emf.b);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.z.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.z.b);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_est.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_est.b);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.v_err.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.v_err.b);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_err.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.i_err.b);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.sign.a);
TEST_ASSERT_EQUAL_FLOAT(0.0, ao.sign.b);
}
/* Feed SMO observer with zeros */
static void test_observer_smo_zeros(void)
{
struct motor_sobserver_dummy_s so;
struct motor_observer_smo_s ao;
struct motor_observer_s o;
struct motor_phy_params_s phy;
ab_frame_t i_ab = {0.0, 0.0};
ab_frame_t v_ab = {0.0, 0.0};
float k_slide = 1.0;
float err_max = 1.0;
float per = 1e-6;
float angle = 0.0;
int i = 0;
/* Initialize motor physical parameters */
phy.p = 8;
phy.res = 0.001;
phy.ind = 10e-6;
/* Initialize SMO angle observer */
motor_observer_smo_init(&ao, k_slide, err_max);
/* Initialize observer */
motor_observer_init(&o, &ao, &so, per);
/* Feed SMO observer with zeros. This should return
* angle equal to observer phase shift (direction * PI)
*/
i_ab.a = 0.0;
i_ab.b = 0.0;
v_ab.a = 0.0;
v_ab.b = 0.0;
for (i = 0; i< 1000000; i+=1)
{
motor_observer_smo(&o, &i_ab, &v_ab, &phy, DIR_CW);
}
angle = motor_observer_angle_get(&o);
/* Test */
TEST_ASSERT_EQUAL_FLOAT(M_PI_F, angle);
for (i = 0; i< 1000000; i+=1)
{
motor_observer_smo(&o, &i_ab, &v_ab, &phy, DIR_CCW);
}
angle = motor_observer_angle_get(&o);
/* Test. NOTE: -M_PI_F normalised = 0.0 */
TEST_ASSERT_FLOAT_WITHIN(5e-6, 0.0, angle);
}
/* Feed SMO in CW direction */
static void test_observer_smo_cw(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/* Feed SMO in CCW direction */
static void test_observer_smo_ccw(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/* SMO observer in linear region */
static void test_observer_smo_linear(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/* SMO observer gain saturation */
static void test_observer_smo_gain_saturate(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/* DIV speed observer initialization */
static void test_sobserver_div_init(void)
{
struct motor_sobserver_div_s so;
uint8_t samples = 10;
float filter = 0.1;
float per = 10e-6;
/* Initialize DIV speed observer */
motor_sobserver_div_init(&so, samples, filter, per);
TEST_ASSERT_EQUAL_FLOAT(filter, so.filter);
TEST_ASSERT_EQUAL_FLOAT(1.0/(samples*per), so.one_by_dt);
TEST_ASSERT_EQUAL_UINT8(samples, so.samples);
}
/* Feed DIV speed observer with zeros */
static void test_sobserver_div_zeros(void)
{
struct motor_sobserver_div_s so;
struct motor_observer_s o;
struct motor_observer_dummy_s ao;
uint8_t samples = 10;
float filter = 0.1;
int i = 0;
float expected_s = 0.0;
float speed = 0.0;
float per = 1e-6;
/* Initialize DIV speed observer */
motor_sobserver_div_init(&so, samples, filter, per);
/* Initialize observer */
motor_observer_init(&o, &ao, &so, per);
/* Feed observer with zeros in CW direction */
expected_s = 0.0;
for(i = 0; i < 1000; i += 1)
{
motor_sobserver_div(&o, 0.0, DIR_CW);
}
speed = motor_observer_speed_get(&o);
/* Test */
TEST_ASSERT_EQUAL_FLOAT(expected_s, speed);
/* Feed observer with zeros in CCW direction */
expected_s = 0.0;
for(i = 0; i < 1000; i += 1)
{
motor_sobserver_div(&o, 0.0, DIR_CCW);
}
speed = motor_observer_speed_get(&o);
/* Test */
TEST_ASSERT_EQUAL_FLOAT(expected_s, speed);
}
/* Feed DIV speed observer in CW direction */
static void test_sobserver_div_cw(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/* Feed DIV speed observer in CCW direction */
static void test_sobserver_div_ccw(void)
{
/* TODO */
TEST_FAIL_MESSAGE("not implemented");
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: test_observer
****************************************************************************/
void test_observer(void)
{
UNITY_BEGIN();
TEST_SEPARATOR();
/* Test observer functions */
RUN_TEST(test_observer_init);
/* SMO observer */
RUN_TEST(test_observer_smo_init);
RUN_TEST(test_observer_smo_zeros);
RUN_TEST(test_observer_smo_linear);
RUN_TEST(test_observer_smo_gain_saturate);
RUN_TEST(test_observer_smo_cw);
RUN_TEST(test_observer_smo_ccw);
/* DIV observer */
RUN_TEST(test_sobserver_div_init);
RUN_TEST(test_sobserver_div_zeros);
RUN_TEST(test_sobserver_div_cw);
RUN_TEST(test_sobserver_div_ccw);
UNITY_END();
}

516
examples/dsptest/test_pid.c Normal file
View File

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

310
examples/dsptest/test_svm.c Normal file
View File

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

303
examples/dsptest/test_transform.c Normal file
View File

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