sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
e0dcfa0c55
nuttx-apps/examples/dsptest/test_pid.c
Mateusz Szafoni e66470df68 Merged in raiden00/apps (pull request #143) 
initial commit for examples/dsptest - unit test for Nuttx DSP library

Approved-by: GregoryN <gnutt@nuttx.org>
2018-07-07 17:09:43 +00:00

517 lines
11 KiB
C
Raw Blame History

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