arm_pid_init_q15.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_q15.c  
00009 *  
00010 * Description:  Q15 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  * @details  
00036  * @param[in,out] *S points to an instance of the Q15 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_q15(  
00048   arm_pid_instance_q15 * S,  
00049   int32_t resetStateFlag)  
00050 {  
00051   /* Derived coefficient A0 */  
00052   S->A0 = __QADD16(__QADD16(S->Kp, S->Ki), S->Kd);  
00053   
00054   /* Derived coefficients and pack into A1 */  
00055   S->A1 = __PKHBT(-__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), S->Kd, 16);  
00056   
00057   /* Check whether state needs reset or not */  
00058   if(resetStateFlag) 
00059   {  
00060     /* Clear the state buffer.  The size will be always 3 samples */  
00061     memset(S->state, 0, 3u * sizeof(q15_t));  
00062   }  
00063   
00064 }  
00065   
00066 /**  
00067  * @} end of PID group  
00068  */