| /**************************************************************************** |
| * 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); |
| } |
| |
| /* Process 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(); |
| } |
