From 9c59a184dba820975e5da6fcd5d248aee87f7e2f Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Sun, 2 Feb 2025 11:26:53 -0500 Subject: add l476 implementation --- .../Source/ControllerFunctions/arm_pid_init_q31.c | 184 ++++++++++----------- 1 file changed, 92 insertions(+), 92 deletions(-) (limited to 'Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.c') diff --git a/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.c b/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.c index 53df5ee..680d463 100644 --- a/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.c +++ b/Drivers/CMSIS/DSP/Source/ControllerFunctions/arm_pid_init_q31.c @@ -1,92 +1,92 @@ -/* ---------------------------------------------------------------------- - * Project: CMSIS DSP Library - * Title: arm_pid_init_q31.c - * Description: Q31 PID Control initialization function - * - * $Date: 23 April 2021 - * $Revision: V1.9.0 - * - * Target Processor: Cortex-M and Cortex-A cores - * -------------------------------------------------------------------- */ -/* - * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved. - * - * SPDX-License-Identifier: Apache-2.0 - * - * Licensed under the Apache License, Version 2.0 (the License); you may - * not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an AS IS BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "dsp/controller_functions.h" - -/** - @addtogroup PID - @{ - */ - -/** - @brief Initialization function for the Q31 PID Control. - @param[in,out] S points to an instance of the Q31 PID structure - @param[in] resetStateFlag - - value = 0: no change in state - - value = 1: reset state - @return none - - @par Details - The resetStateFlag specifies whether to set state to zero or not. \n - The function computes the structure fields: A0, A1 A2 - using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd) - also sets the state variables to all zeros. - */ - -void arm_pid_init_q31( - arm_pid_instance_q31 * S, - int32_t resetStateFlag) -{ - -#if defined (ARM_MATH_DSP) - - /* Derived coefficient A0 */ - S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd); - - /* Derived coefficient A1 */ - S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp); - -#else - - q31_t temp; /* to store the sum */ - - /* Derived coefficient A0 */ - temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki); - S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd); - - /* Derived coefficient A1 */ - temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd); - S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp); - -#endif /* #if defined (ARM_MATH_DSP) */ - - /* Derived coefficient A2 */ - S->A2 = S->Kd; - - /* Check whether state needs reset or not */ - if (resetStateFlag) - { - /* Reset state to zero, The size will be always 3 samples */ - memset(S->state, 0, 3U * sizeof(q31_t)); - } - -} - -/** - @} end of PID group - */ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_pid_init_q31.c + * Description: Q31 PID Control initialization function + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + @addtogroup PID + @{ + */ + +/** + @brief Initialization function for the Q31 PID Control. + @param[in,out] S points to an instance of the Q31 PID structure + @param[in] resetStateFlag + - value = 0: no change in state + - value = 1: reset state + @return none + + @par Details + The resetStateFlag specifies whether to set state to zero or not. \n + The function computes the structure fields: A0, A1 A2 + using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd) + also sets the state variables to all zeros. + */ + +void arm_pid_init_q31( + arm_pid_instance_q31 * S, + int32_t resetStateFlag) +{ + +#if defined (ARM_MATH_DSP) + + /* Derived coefficient A0 */ + S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd); + + /* Derived coefficient A1 */ + S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp); + +#else + + q31_t temp; /* to store the sum */ + + /* Derived coefficient A0 */ + temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki); + S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd); + + /* Derived coefficient A1 */ + temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd); + S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp); + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Derived coefficient A2 */ + S->A2 = S->Kd; + + /* Check whether state needs reset or not */ + if (resetStateFlag) + { + /* Reset state to zero, The size will be always 3 samples */ + memset(S->state, 0, 3U * sizeof(q31_t)); + } + +} + +/** + @} end of PID group + */ -- cgit v1.2.3