sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
d614ee8efa
nuttx-apps/examples/dsptest/test_misc.c

634 lines
14 KiB
C
Raw Blame History

/****************************************************************************
* 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 functions */
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();
}
Reference in New Issue View Git Blame Copy Permalink