arm_pid_init_q31.c

00001 /* ----------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
00006 *  
00007 * Project:      CMSIS DSP Library  
00008 * Title:        arm_pid_init_q31.c  
00009 *  
00010 * Description:  Q31 PID Control initialization function   
00011 *  
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated.  
00025 * ------------------------------------------------------------------- */ 
00026  
00027 #include "arm_math.h" 
00028  
00029  /**  
00030  * @addtogroup PID  
00031  * @{  
00032  */ 
00033  
00034 /**  
00035  * @brief  Initialization function for the Q31 PID Control. 
00036  * @param[in,out] *S points to an instance of the Q31 PID structure. 
00037  * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state. 
00038  * @return none.  
00039  * \par Description: 
00040  * \par  
00041  * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n 
00042  * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>  
00043  * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)  
00044  * also sets the state variables to all zeros.  
00045  */  
00046   
00047 void arm_pid_init_q31(  
00048   arm_pid_instance_q31 * S,  
00049   int32_t resetStateFlag) 
00050 {  
00051   /* Derived coefficient A0 */  
00052   S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);  
00053   
00054   /* Derived coefficient A1 */  
00055   S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);  
00056   
00057   /* Derived coefficient A2 */  
00058   S->A2 = S->Kd;  
00059   
00060   /* Check whether state needs reset or not */  
00061   if(resetStateFlag) 
00062   {  
00063     /* Clear the state buffer.  The size will be always 3 samples */  
00064     memset(S->state, 0, 3u * sizeof(q31_t));  
00065   }  
00066   
00067 }  
00068   
00069 /**  
00070  * @} end of PID group  
00071  */