diff --git a/module/CMakeLists.txt b/module/CMakeLists.txt index 7ac162a925b67d6230e7ad52c8d69d0e4fdbdc26..325246b2f1f0b0e556514f8434b4a59696a7fc81 100644 --- a/module/CMakeLists.txt +++ b/module/CMakeLists.txt @@ -52,6 +52,7 @@ list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/msys_rom") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/noc_s3") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/pcid") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/perf_controller") +list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/pid_controller") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/pik_clock") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/pl011") list(APPEND SCP_MODULE_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/power_capping") diff --git a/module/pid_controller/CMakeLists.txt b/module/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..cf446a6f4c34a4c995c969fe5e027e841f136234 --- /dev/null +++ b/module/pid_controller/CMakeLists.txt @@ -0,0 +1,14 @@ +# +# Arm SCP/MCP Software +# Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# + +add_library(${SCP_MODULE_TARGET} SCP_MODULE) + +target_include_directories(${SCP_MODULE_TARGET} + PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") + +target_sources(${SCP_MODULE_TARGET} + PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/src/mod_pid_controller.c") diff --git a/module/pid_controller/Module.cmake b/module/pid_controller/Module.cmake new file mode 100644 index 0000000000000000000000000000000000000000..56cc9aea9095d41e4f79dfce8ee3012d7d52ca60 --- /dev/null +++ b/module/pid_controller/Module.cmake @@ -0,0 +1,9 @@ +# +# Arm SCP/MCP Software +# Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# + +set(SCP_MODULE "pid-controller") +set(SCP_MODULE_TARGET "module-pid-controller") diff --git a/module/pid_controller/include/mod_pid_controller.h b/module/pid_controller/include/mod_pid_controller.h new file mode 100644 index 0000000000000000000000000000000000000000..86210eec40ed73b737625d98edb93a6ccf7e3b4b --- /dev/null +++ b/module/pid_controller/include/mod_pid_controller.h @@ -0,0 +1,196 @@ +/* + * Arm SCP/MCP Software + * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + * + * Description: + * PID Controller. + */ + +#ifndef MOD_PID_CONTROLLER_H +#define MOD_PID_CONTROLLER_H + +#include +#include +#include + +#include + +/*! + * \ingroup GroupModules + * \defgroup GroupPIDController PID Controller + * + * \details Generic PID Controller module. + * + * \{ + */ + +/*! + * \brief Constant parameter structure for PID controller. + * + * \details This structure defines a PID controller parameter, where the + * numerator represents the term's value, and the divisor indicates + * a power-of-2 shift for scaling. This allows for fractional precision + * through bit-shifting, avoiding floating point operations. + */ +struct mod_pid_controller_k_parameter { + /*! Numerator value */ + int32_t numerator; + + /*! Power-of-2 shift to scale the term value (2^divisor_shift) */ + uint8_t divisor_shift; +}; + +/*! + * \brief PID controller element configuration structure. + */ +struct mod_pid_controller_elem_config { + /*! + * \brief Control set point value + * + * \details The value that the system will achive once stabilised. + */ + int64_t set_point; + + /*! + * \brief Switch-on value threshold. + * + * \details The value above which the PID loop runs. Below this + * threshold the output is allocated only on bias coefficients. + */ + int64_t switch_on_value; + + /*! Desired output when the controller is not active */ + int64_t inactive_state_output; + + /*! + * \brief Integral cut-off threshold. + * + * \details The integral term will accumulate errors only when the + * control input is within a specified range. This helps to prevent + * the integral wind-up by stopping the accumulation of errors + * when the control input is saturated or outside the desired + * operating range. + */ + int64_t integral_cutoff; + + /*! + * \brief Integral maximum. + * + * \details This is the upper limit the accumulated errors. + */ + int64_t integral_max; + + /* Controller constant parameters */ + + /*! + * \brief Controller constant parameters structure. + * + * \details This structure defines the constant parameters for a PID + * controller, where each term is represented with both a value and a + * corresponding denominator. The denominators are used as + * power-of-2 shifts to scale down their respective terms, allowing + * fractional precision without using floating point calculations. + * + * The output of the controller is calculated as: + * output = (Kp * e) / (2^Kp_d_shift) + + * (Ki * ei) / (2^Ki_d_shift) + + * (Kd * ed) / (2^Kd_d_shift) + */ + struct { + /*! Proportional term when undershooting */ + struct mod_pid_controller_k_parameter proportional_undershoot; + + /*! Proportional term when overshooting */ + struct mod_pid_controller_k_parameter proportional_overshoot; + + /*! Integral term */ + struct mod_pid_controller_k_parameter integral; + + /*! Derivative term */ + struct mod_pid_controller_k_parameter derivative; + } k; + + /*! Output limits */ + struct { + /*! Minimum output limit */ + int64_t min; + + /*! Maximum output limit */ + int64_t max; + } output; +}; + +/*! + * \brief PID Controller API + * + * \details API for the PID controller to update and set control values. + */ +struct mod_pid_controller_api { + /*! + * \brief Update the PID controller with a new input value. + * + * \details It is crucial that sampling is performed periodically at the + * same time rate (at consistent intervals), because the PID + * controller relies on the time rate to compute the proportional, + * integral and derivative terms accurately. + * + * \param id The identifier of the PID controller. + * \param input The input value to update the controller with. + * \param[out] output The output value calculated by the controller. + * + * \retval ::FWK_SUCCESS If the call is successful. + * \retval ::FWK_E_DATA Integral error overflow occurred. + * \return One of the standard framework error codes. + */ + int (*update)(fwk_id_t id, int64_t input, int64_t *output); + + /*! + * \brief Set the control value for the PID controller. + * + * \param id The identifier of the PID controller. + * \param input The control value to be set. + * + * \retval ::FWK_SUCCESS If the call is successful. + * \retval ::FWK_E_PARAM Control value under switch on value. + * \return One of the standard framework error codes. + */ + int (*set_point)(fwk_id_t id, int64_t input); + + /*! + * \brief Reset the PID controller. + * + * \details This function resets the internal state of the PID controller, + * including the accumulated integral term and any other relevant + * state variables. + * + * \param id The identifier of the PID controller. + * + * \retval ::FWK_SUCCESS If the reset is successful. + * \return One of the standard framework error codes. + */ + int (*reset)(fwk_id_t id); +}; + +/*! + * \brief API indices. + */ +enum mod_pid_controller_api_idx { + /*! Index for control API. */ + MOD_PID_CONTROLLER_API_IDX_CONTROL, + + /*! Number of defined APIs. */ + MOD_PID_CONTROLLER_API_IDX_COUNT, +}; + +/*! + * \brief Module API identifier. + */ +extern const fwk_id_t mod_pid_controller_api_id_control; + +/*! + * \} + */ + +#endif /* MOD_PID_CONTROLLER_H */ diff --git a/module/pid_controller/src/mod_pid_controller.c b/module/pid_controller/src/mod_pid_controller.c new file mode 100644 index 0000000000000000000000000000000000000000..67c77e302db3204215b882467262ed532a1c6284 --- /dev/null +++ b/module/pid_controller/src/mod_pid_controller.c @@ -0,0 +1,286 @@ +/* + * Arm SCP/MCP Software + * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#define MOD_NAME "[PID_CTRL] " + +/* Module API identifier */ +const fwk_id_t mod_pid_controller_api_id_control = FWK_ID_API_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, + MOD_PID_CONTROLLER_API_IDX_CONTROL); + +/* Element Context */ +struct mod_pid_controller_elem_ctx { + /* Pointer to configuration data */ + const struct mod_pid_controller_elem_config *config; + + struct { + /* Integral (accumulated) error */ + int64_t integral; + + /* Derivative error */ + int64_t derivative; + + /* Error from previous iteration */ + int64_t previous; + + /* Current error */ + int64_t current; + } error; + + /* Control value */ + int64_t set_point; +}; + +/* Module Context */ +struct mod_pid_contorller_ctx { + /* Table of element context */ + struct mod_pid_controller_elem_ctx *elem_ctx_table; + + /* Number of elements */ + unsigned int element_count; +}; + +static struct mod_pid_contorller_ctx mod_ctx; + +/* + * Helper functions. + */ + +static struct mod_pid_controller_elem_ctx *get_elem_ctx(fwk_id_t elem_id) +{ + unsigned int idx; + + idx = fwk_id_get_element_idx(elem_id); + if (idx >= mod_ctx.element_count) { + return NULL; + } + + return &mod_ctx.elem_ctx_table[idx]; +} + +static inline void reset_error_values( + struct mod_pid_controller_elem_ctx *pid_ctx) +{ + memset(&pid_ctx->error, 0, sizeof(pid_ctx->error)); +} + +static inline bool is_integral_error_overflow( + struct mod_pid_controller_elem_ctx *pid_ctx) +{ + int64_t current = pid_ctx->error.current; + int64_t integral = pid_ctx->error.integral; + + return (current > 0 && integral > (INT64_MAX - current)) || + (current < 0 && integral < (INT64_MIN - current)); +} + +static void integrate_error_with_anti_windup( + struct mod_pid_controller_elem_ctx *pid_ctx) +{ + int64_t accumulated_error = + pid_ctx->error.integral + pid_ctx->error.current; + + if ((pid_ctx->error.current < pid_ctx->config->integral_cutoff) && + (accumulated_error < pid_ctx->config->integral_max)) { + /* + * The error is below the cutoff value and, + * the accumulated error is still within the maximum permissible + * value, thus continue integration. + */ + pid_ctx->error.integral = accumulated_error; + } +} + +static void calculate_derivate_error( + struct mod_pid_controller_elem_ctx *pid_ctx) +{ + pid_ctx->error.derivative = + pid_ctx->error.current - pid_ctx->error.previous; + pid_ctx->error.previous = pid_ctx->error.current; +} + +static inline const struct mod_pid_controller_k_parameter *get_k_prop( + struct mod_pid_controller_elem_ctx *pid_ctx) +{ + if (pid_ctx->error.current < 0) { + return &pid_ctx->config->k.proportional_overshoot; + } + + return &pid_ctx->config->k.proportional_undershoot; +} + +static inline int64_t compute_term( + int64_t error, + const struct mod_pid_controller_k_parameter *k) +{ + return (error * (int64_t)k->numerator) >> k->divisor_shift; +} + +static int64_t calculate_output(struct mod_pid_controller_elem_ctx *pid_ctx) +{ + int64_t pid_output; + + pid_output = compute_term(pid_ctx->error.current, get_k_prop(pid_ctx)) + + compute_term(pid_ctx->error.integral, &pid_ctx->config->k.integral) + + compute_term(pid_ctx->error.derivative, &pid_ctx->config->k.derivative); + + /* Constrain output to boundaries */ + pid_output = FWK_MAX(pid_output, pid_ctx->config->output.min); + pid_output = FWK_MIN(pid_output, pid_ctx->config->output.max); + + return pid_output; +} + +/* + * API functions. + */ + +static int pid_controller_update(fwk_id_t id, int64_t input, int64_t *output) +{ + struct mod_pid_controller_elem_ctx *pid_ctx; + + fwk_assert(output != NULL); + + pid_ctx = get_elem_ctx(id); + fwk_assert(pid_ctx != NULL); + + if (input < pid_ctx->config->switch_on_value) { + reset_error_values(pid_ctx); + *output = pid_ctx->config->inactive_state_output; + return FWK_SUCCESS; + } + + pid_ctx->error.current = pid_ctx->set_point - input; + if (is_integral_error_overflow(pid_ctx)) { + FWK_LOG_ERR(MOD_NAME "Integral error overflow"); + return FWK_E_DATA; + } + + /* Calculate the integral term */ + integrate_error_with_anti_windup(pid_ctx); + + /* Calculate the derivative term */ + calculate_derivate_error(pid_ctx); + + *output = calculate_output(pid_ctx); + + return FWK_SUCCESS; +} + +static int pid_controller_set_point(fwk_id_t id, int64_t input) +{ + struct mod_pid_controller_elem_ctx *pid_ctx; + + pid_ctx = get_elem_ctx(id); + fwk_assert(pid_ctx != NULL); + + if (input < pid_ctx->config->switch_on_value) { + FWK_LOG_ERR(MOD_NAME "Error set control value under switch on value."); + return FWK_E_PARAM; + } + + pid_ctx->set_point = input; + reset_error_values(pid_ctx); + + return FWK_SUCCESS; +} + +static int pid_controller_reset(fwk_id_t id) +{ + struct mod_pid_controller_elem_ctx *pid_ctx; + + pid_ctx = get_elem_ctx(id); + fwk_assert(pid_ctx != NULL); + + reset_error_values(pid_ctx); + + return FWK_SUCCESS; +} + +const struct mod_pid_controller_api pid_controller_api = { + .update = pid_controller_update, + .set_point = pid_controller_set_point, + .reset = pid_controller_reset, +}; + +/* + * Framework handler functions. + */ + +static int pid_controller_init( + fwk_id_t module_id, + unsigned int element_count, + const void *data) +{ + mod_ctx.elem_ctx_table = fwk_mm_calloc( + element_count, sizeof(struct mod_pid_controller_elem_ctx)); + mod_ctx.element_count = element_count; + + return FWK_SUCCESS; +} + +static int pid_controller_element_init( + fwk_id_t element_id, + unsigned int unused, + const void *data) +{ + int status; + struct mod_pid_controller_elem_ctx *elem_ctx; + const struct mod_pid_controller_elem_config *config = + (const struct mod_pid_controller_elem_config *)data; + + elem_ctx = get_elem_ctx(element_id); + fwk_assert(elem_ctx != NULL); + elem_ctx->config = config; + status = pid_controller_set_point(element_id, config->set_point); + + return status; +} + +int pid_controller_bind_request( + fwk_id_t source_id, + fwk_id_t target_id, + fwk_id_t api_id, + const void **api) +{ + enum mod_pid_controller_api_idx api_idx; + int status; + + api_idx = (enum mod_pid_controller_api_idx)fwk_id_get_api_idx(api_id); + switch (api_idx) { + case MOD_PID_CONTROLLER_API_IDX_CONTROL: + *api = &pid_controller_api; + status = FWK_SUCCESS; + break; + + default: + status = FWK_E_PARAM; + break; + } + + return status; +} + +const struct fwk_module module_pid_controller = { + .type = FWK_MODULE_TYPE_SERVICE, + .event_count = (unsigned int)0, + .api_count = (unsigned int)MOD_PID_CONTROLLER_API_IDX_COUNT, + .init = pid_controller_init, + .element_init = pid_controller_element_init, + .process_bind_request = pid_controller_bind_request, +}; diff --git a/module/pid_controller/test/CMakeLists.txt b/module/pid_controller/test/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..dcaa118b906984f76e5e121cf7adcbdb0d977309 --- /dev/null +++ b/module/pid_controller/test/CMakeLists.txt @@ -0,0 +1,22 @@ +# +# Arm SCP/MCP Software +# Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# + +set(TEST_SRC mod_pid_controller) +set(TEST_FILE mod_pid_controller) + +set(UNIT_TEST_TARGET mod_${TEST_MODULE}_unit_test) + +set(MODULE_SRC ${MODULE_ROOT}/${TEST_MODULE}/src) +set(MODULE_INC ${MODULE_ROOT}/${TEST_MODULE}/include) +set(MODULE_UT_SRC ${CMAKE_CURRENT_LIST_DIR}) +set(MODULE_UT_INC ${CMAKE_CURRENT_LIST_DIR}) + +list(APPEND MOCK_REPLACEMENTS fwk_module) +list(APPEND MOCK_REPLACEMENTS fwk_id) +list(APPEND MOCK_REPLACEMENTS fwk_mm) + +include(${SCP_ROOT}/unit_test/module_common.cmake) diff --git a/module/pid_controller/test/fwk_module_idx.h b/module/pid_controller/test/fwk_module_idx.h new file mode 100644 index 0000000000000000000000000000000000000000..b7b17ea10f5edc1516b0945a25bf9b54d362d153 --- /dev/null +++ b/module/pid_controller/test/fwk_module_idx.h @@ -0,0 +1,25 @@ +/* + * Arm SCP/MCP Software + * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef TEST_FWK_MODULE_MODULE_IDX_H +#define TEST_FWK_MODULE_MODULE_IDX_H + +#include + +enum fwk_module_idx { + FWK_MODULE_IDX_PID_CONTROLLER, + FWK_MODULE_IDX_FAKE_0, + FWK_MODULE_IDX_COUNT, +}; + +static const fwk_id_t fwk_module_id_pid_controller = + FWK_ID_MODULE_INIT(FWK_MODULE_IDX_PID_CONTROLLER); + +static const fwk_id_t fwk_module_id_fake_0 = + FWK_ID_MODULE_INIT(FWK_MODULE_IDX_FAKE_0); + +#endif /* TEST_FWK_MODULE_MODULE_IDX_H */ diff --git a/module/pid_controller/test/mod_pid_controller_unit_test.c b/module/pid_controller/test/mod_pid_controller_unit_test.c new file mode 100644 index 0000000000000000000000000000000000000000..79b76cd937d44b7c0ab00e7a5d8767ca7d57da29 --- /dev/null +++ b/module/pid_controller/test/mod_pid_controller_unit_test.c @@ -0,0 +1,476 @@ +/* + * Arm SCP/MCP Software + * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + * + * Description: + * PID Controller unit tests. + */ + +#include "scp_unity.h" +#include "unity.h" + +#include +#include +#include + +#include + +#include + +#include UNIT_TEST_SRC + +enum mod_pid_elem_id { + PID_CONTROLLER_FAKE_INDEX_0, + PID_CONTROLLER_FAKE_INDEX_COUNT, +}; + +static void pid_controller_init_one_element( + struct mod_pid_controller_elem_ctx *mock_elem_ctx, + struct mod_pid_controller_elem_config *mock_config) +{ + mod_ctx.element_count = PID_CONTROLLER_FAKE_INDEX_COUNT; + mod_ctx.elem_ctx_table = mock_elem_ctx; + mock_elem_ctx->config = mock_config; +} + +void setUp(void) +{ +} + +void tearDown(void) +{ +} + +static void test_pid_controller_init_success(void) +{ + struct mod_pid_controller_elem_ctx *return_table = + (struct mod_pid_controller_elem_ctx *)0x7A7A7A7A; + unsigned int element_count = 1; + int status; + + fwk_mm_calloc_ExpectAndReturn( + element_count, + sizeof(struct mod_pid_controller_elem_ctx), + (void *)return_table); + + status = + pid_controller_init(fwk_module_id_pid_controller, element_count, NULL); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL_PTR(return_table, mod_ctx.elem_ctx_table); + TEST_ASSERT_EQUAL(element_count, mod_ctx.element_count); +} + +static void test_pid_controller_element_init_success(void) +{ + fwk_id_t element_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config = { .set_point = 42 }; + unsigned int unused = 0; + int status; + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + fwk_id_get_element_idx_ExpectAndReturn( + element_id, PID_CONTROLLER_FAKE_INDEX_0); + fwk_id_get_element_idx_ExpectAndReturn( + element_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_element_init( + element_id, unused, (const void *)&mock_config); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL_PTR(&mock_config, mock_elem_ctx.config); +} + +static void test_pid_controller_bind_request_api_control(void) +{ + fwk_id_t source_id = FWK_ID_ELEMENT_INIT(FWK_MODULE_IDX_FAKE_0, 0); + fwk_id_t target_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + fwk_id_t api_id = FWK_ID_API_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, MOD_PID_CONTROLLER_API_IDX_CONTROL); + const void *api = NULL; + int status; + + fwk_id_get_api_idx_ExpectAndReturn( + api_id, MOD_PID_CONTROLLER_API_IDX_CONTROL); + + status = pid_controller_bind_request(source_id, target_id, api_id, &api); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL_PTR(&pid_controller_api, api); +} + +static void test_pid_controller_bind_request_api_invalid_id(void) +{ + fwk_id_t source_id = FWK_ID_ELEMENT_INIT(FWK_MODULE_IDX_FAKE_0, 0); + fwk_id_t target_id = FWK_ID_ELEMENT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + fwk_id_t api_id = FWK_ID_API( + FWK_MODULE_IDX_PID_CONTROLLER, MOD_PID_CONTROLLER_API_IDX_CONTROL); + const void *api = NULL; + int status; + + fwk_id_get_api_idx_ExpectAndReturn( + api_id, MOD_PID_CONTROLLER_API_IDX_COUNT); + + status = pid_controller_bind_request(source_id, target_id, api_id, &api); + + TEST_ASSERT_EQUAL(FWK_E_PARAM, status); + TEST_ASSERT_NULL(api); +} + +static void test_get_elem_ctx_valid_id(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + pid_controller_init_one_element(&mock_elem_ctx, NULL); + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + struct mod_pid_controller_elem_ctx *ctx = get_elem_ctx(mock_id); + TEST_ASSERT_EQUAL_PTR(&mock_elem_ctx, ctx); +} + +static void test_get_elem_ctx_invalid_id(void) +{ + mod_ctx.element_count = PID_CONTROLLER_FAKE_INDEX_COUNT; + + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_COUNT); + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_COUNT); + + struct mod_pid_controller_elem_ctx *ctx = get_elem_ctx(mock_id); + TEST_ASSERT_NULL(ctx); +} + +static void test_reset_error_values(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + + mock_elem_ctx.error.integral = 1; + mock_elem_ctx.error.derivative = 2; + mock_elem_ctx.error.current = 3; + mock_elem_ctx.error.previous = 4; + + reset_error_values(&mock_elem_ctx); + + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.integral); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.derivative); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.current); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.previous); +} + +static void test_is_integral_error_overflow_no_overflow(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + bool overflow; + + mock_elem_ctx.error.current = 5; + mock_elem_ctx.error.integral = INT64_MAX - 6; + + overflow = is_integral_error_overflow(&mock_elem_ctx); + + TEST_ASSERT_FALSE(overflow); +} + +static void test_is_integral_error_overflow(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + bool overflow; + + mock_elem_ctx.error.current = 5; + mock_elem_ctx.error.integral = INT64_MAX - 2; + + overflow = is_integral_error_overflow(&mock_elem_ctx); + + TEST_ASSERT_TRUE(overflow); + + mock_elem_ctx.error.current = -5; + mock_elem_ctx.error.integral = INT64_MIN + 2; + + overflow = is_integral_error_overflow(&mock_elem_ctx); + + TEST_ASSERT_TRUE(overflow); +} + +static void test_get_k_prop(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + const struct mod_pid_controller_k_parameter *k_prop; + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_elem_ctx.error.current = -1; + k_prop = get_k_prop(&mock_elem_ctx); + TEST_ASSERT_EQUAL_PTR(&mock_config.k.proportional_overshoot, k_prop); + + mock_elem_ctx.error.current = 1; + k_prop = get_k_prop(&mock_elem_ctx); + TEST_ASSERT_EQUAL_PTR(&mock_config.k.proportional_undershoot, k_prop); +} + +static void test_pid_controller_calculate_output(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t output; + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_config.output.min = -100; + mock_config.output.max = 100; + memset(&mock_config.k, 0, sizeof(mock_config.k)); + mock_config.k.proportional_overshoot.numerator = 1; + mock_config.k.proportional_undershoot.numerator = 1; + mock_config.k.integral.numerator = 1; + mock_config.k.derivative.numerator = 1; + mock_elem_ctx.error.current = 1; + mock_elem_ctx.error.integral = 1; + mock_elem_ctx.error.derivative = 1; + + output = calculate_output(&mock_elem_ctx); + TEST_ASSERT_EQUAL(3, output); +} + +static void test_pid_controller_calculate_output_with_constraints(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t output; + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_config.output.min = -2; + mock_config.output.max = 2; + memset(&mock_config.k, 0, sizeof(mock_config.k)); + mock_config.k.proportional_overshoot.numerator = 1; + mock_config.k.proportional_undershoot.numerator = 1; + mock_config.k.integral.numerator = 1; + mock_config.k.derivative.numerator = 1; + mock_elem_ctx.error.current = 1; + mock_elem_ctx.error.integral = 1; + mock_elem_ctx.error.derivative = 1; + + output = calculate_output(&mock_elem_ctx); + TEST_ASSERT_EQUAL(mock_config.output.max, output); + + mock_elem_ctx.error.current = -1; + mock_elem_ctx.error.integral = -1; + mock_elem_ctx.error.derivative = -1; + + output = calculate_output(&mock_elem_ctx); + TEST_ASSERT_EQUAL(mock_config.output.min, output); +} + +static void test_pid_controller_update_inactive_output(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t input, output; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_config.switch_on_value = 50; + mock_config.inactive_state_output = 20; + input = mock_config.switch_on_value - 1; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_update(mock_id, input, &output); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL(mock_config.inactive_state_output, output); +} + +static void test_pid_controller_update_integral_overflow(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t input, output; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_config.switch_on_value = 50; + input = mock_config.switch_on_value + 1; + mock_elem_ctx.set_point = input + 1; + mock_elem_ctx.error.integral = + INT64_MAX - (mock_elem_ctx.set_point - input) + 1; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_update(mock_id, input, &output); + + TEST_ASSERT_EQUAL(FWK_E_DATA, status); +} + +static void test_pid_controller_update_output_calculation(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t input, output; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + + mock_config.output.min = -100; + mock_config.output.max = 100; + mock_config.integral_max = 100; + mock_config.integral_cutoff = 100; + mock_config.switch_on_value = 50; + memset(&mock_config.k, 0, sizeof(mock_config.k)); + mock_config.k.proportional_overshoot.numerator = 1; + mock_config.k.proportional_undershoot.numerator = 1; + mock_config.k.integral.numerator = 1; + mock_config.k.derivative.numerator = 1; + input = mock_config.switch_on_value + 1; + mock_elem_ctx.set_point = input + 1; + + mock_elem_ctx.error.integral = 0; + mock_elem_ctx.error.previous = 0; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_update(mock_id, input, &output); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL(3, output); +} + +static void test_pid_controller_set_point_valid_input(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t new_set_point; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + new_set_point = mock_config.switch_on_value + 1; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_set_point(mock_id, new_set_point); + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL(new_set_point, mock_elem_ctx.set_point); +} + +static void test_pid_controller_set_point_equal_to_switch_on_value(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t new_set_point; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + new_set_point = mock_elem_ctx.config->switch_on_value; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_set_point(mock_id, new_set_point); + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL(new_set_point, mock_elem_ctx.set_point); +} + +static void test_pid_controller_set_point_below_to_switch_on_value(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + struct mod_pid_controller_elem_config mock_config; + int64_t new_set_point, old_set_point; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, &mock_config); + new_set_point = mock_elem_ctx.config->switch_on_value - 1; + old_set_point = mock_elem_ctx.set_point; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_set_point(mock_id, new_set_point); + TEST_ASSERT_EQUAL(FWK_E_PARAM, status); + TEST_ASSERT_EQUAL(old_set_point, mock_elem_ctx.set_point); +} + +static void test_pid_controller_reset(void) +{ + struct mod_pid_controller_elem_ctx mock_elem_ctx; + int status; + fwk_id_t mock_id = FWK_ID_ELEMENT_INIT( + FWK_MODULE_IDX_PID_CONTROLLER, PID_CONTROLLER_FAKE_INDEX_0); + + pid_controller_init_one_element(&mock_elem_ctx, NULL); + mock_elem_ctx.error.integral = 1; + mock_elem_ctx.error.derivative = 2; + mock_elem_ctx.error.current = 3; + mock_elem_ctx.error.previous = 4; + + fwk_id_get_element_idx_ExpectAndReturn( + mock_id, PID_CONTROLLER_FAKE_INDEX_0); + + status = pid_controller_reset(mock_id); + + TEST_ASSERT_EQUAL(FWK_SUCCESS, status); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.integral); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.derivative); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.current); + TEST_ASSERT_EQUAL(0, mock_elem_ctx.error.previous); +} + +int pid_controller_test_main(void) +{ + UNITY_BEGIN(); + + RUN_TEST(test_pid_controller_init_success); + RUN_TEST(test_pid_controller_element_init_success); + RUN_TEST(test_pid_controller_bind_request_api_control); + RUN_TEST(test_pid_controller_bind_request_api_invalid_id); + RUN_TEST(test_get_elem_ctx_valid_id); + RUN_TEST(test_get_elem_ctx_invalid_id); + RUN_TEST(test_reset_error_values); + RUN_TEST(test_is_integral_error_overflow_no_overflow); + RUN_TEST(test_is_integral_error_overflow); + RUN_TEST(test_get_k_prop); + RUN_TEST(test_pid_controller_calculate_output); + RUN_TEST(test_pid_controller_calculate_output_with_constraints); + RUN_TEST(test_pid_controller_update_inactive_output); + RUN_TEST(test_pid_controller_update_integral_overflow); + RUN_TEST(test_pid_controller_update_output_calculation); + RUN_TEST(test_pid_controller_set_point_valid_input); + RUN_TEST(test_pid_controller_set_point_equal_to_switch_on_value); + RUN_TEST(test_pid_controller_set_point_below_to_switch_on_value); + RUN_TEST(test_pid_controller_reset); + + return UNITY_END(); +} + +#if !defined(TEST_ON_TARGET) +int main(void) +{ + return pid_controller_test_main(); +} +#endif diff --git a/unit_test/CMakeLists.txt b/unit_test/CMakeLists.txt index 66cc1ea692cd81005ec92d6527c694c5041f90c6..e61ab14f30dfdc3863aec05d7ea2b6951bf481f4 100644 --- a/unit_test/CMakeLists.txt +++ b/unit_test/CMakeLists.txt @@ -117,6 +117,7 @@ list(APPEND UNIT_MODULE mhu3) list(APPEND UNIT_MODULE mpmm) list(APPEND UNIT_MODULE power_capping) list(APPEND UNIT_MODULE perf_controller) +list(APPEND UNIT_MODULE pid_controller) list(APPEND UNIT_MODULE pl011) list(APPEND UNIT_MODULE power_domain) list(APPEND UNIT_MODULE ppu_v1)