/******************************************************************************
* 
* Copyright (c) 2012 Freescale Semiconductor;
* All Rights Reserved                       
*
******************************************************************************* 
*
* THIS SOFTWARE IS PROVIDED BY FREESCALE "AS IS" AND ANY EXPRESSED 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 FREESCALE OR ITS 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.
*
***************************************************************************//*!
*
* @file      PCLIB_ControllerPIAsm.h
*
* @author    B37570, B36378
* 
* @version   1.0.00.0
* 
* @date      Jan-26-2012
* 			Updated September 2013
* 
* @brief     PID controller algorithm implemented in assembler
*
*******************************************************************************
*
* PID controller algorithm implemented in assembler.
*
******************************************************************************/
#ifndef __PCLIB_CONTROL_PID_ASM_H__
#define __PCLIB_CONTROL_PID_ASM_H__

/******************************************************************************
* Includes
******************************************************************************/

#include "56800E_types.h"

/******************************************************************************
* Constants
******************************************************************************/

/******************************************************************************
* Macros 
******************************************************************************/

#define PCLIB_ControllerPIDAsm(f16Error, pudtControlStruct)	     \
    	PCLIB_ControllerPIDFAsm(f16Error, pudtControlStruct)
	


typedef struct _pclib_control_pid_params_t
{                  
    Frac16 f16ControlOutN_1;				/* Data Input/Output */
    Frac16 f16KA;  							/* Data Input */
    Frac16 f16KB;  							/* Data Input */
    Frac16 f16KC;  							/* Data Input */
    Frac16 f16UpperLimit;					/* Data Input */
    Frac16 f16LowerLimit;              		/* Data Input */ 
	Frac16 f16Delay_x1;						/* Data Input/Output */
	Frac16 f16Delay_x2;						/* Data Input/Output */
} PCLIB_CONTROL_PID_PARAMS_T;

/******************************************************************************
* Global variables
******************************************************************************/

/******************************************************************************
* Global functions
******************************************************************************/

/***************************************************************************//*
*
* @brief    PID controller implemented with final output limited to its limit 
*			values (UpperLimit and LowerLimit).
*
* @remarks 	
*
*                        -----------                 
*                 e(k)  |           |  u(k)           
*               --------|    PID    |---------        
*               		|           |                 
*                	    |           |
*                        ----------- 
*
*	PID controller: Proportional(Kp), Integral(Ki), and Differential(Kd).
*
*   The PID controller algorithm in continuous time domain:   
*      
*                      	   /t                                       
*   y(t) = Kp*e(t) + (Ki)*|x(t)*dt + Kd*(ded/dt ) 
*                        0/             
*
*   Error(e(t)) = SetPoint - Actual = x(t) - input error in continuous time domain,  
*
*	Output(y(t) = Control Output - controller output in continuous time domain,
*			
*   Kp*e(t)  = Kp*x(t) 
*   
*         /t
*   (Ki)*|e(t)*dt =  (Ki/(1-z^-1))*x(t)               
*      0/                       			                                                               
*	                 
* 	Kd*(ded/dt = Kd*(1-z^-1)*x(t)
*
*   					((Kp+Ki+Kd)*x(t) + (-Kp -2*Kd)*x(t)*z^-1 + Kd*x(t)*z^-2)
* Control Output y(t) =  ---------------------------------------------------------
*									(1-z^-1)
*									
* When Kp+Ki+Kd, -2Kp-Kd, and Kd can be simplified as KA, KB, KC as mention below
*
*	Kp + Ki + Kd = KA
*	-Kp - 2Kd    = KB
*	Kd           = KC
*	
*   					(KA*x(t) + KB*x(t)*z^-1 + KC*x(t)*z^-2)
* Control Output y(t) =  ------------------------------------------
*									(1-z^-1)	
*									
*     y(t) = y(t)*z^-1 + KA*x(t) + KB*x(t)*z^-1 + KC*x(t)*z^-2
*     
*     Where 
*     y(t)	    = y(n)   : Control output at step n - in discrete time domain
*     y(t)*z^-1 = y(n-1) : Control previous output at step n - in discrete time domain
*     x(t)      = x(n)   : Error at step n - in discrete time domain
*     x(t)*z^-1 = x(n-1) : Previous error at step n - in discrete time domain
*     x(t)*z^-2 = x(n-2) : Previous to previous error at step n - in discrete time domain									
*									
*      
*       x_max - max range of x(n)                                              
*       y_max - max range of y(n)                                          
*
*    Frac16 f16ControlOutN_1;		 	- Data InputOutput 
*    Frac16 f16KA;  				 	- Data Input 
*    Frac16 f16KB;  				 	- Data Input 
*    Frac16 f16KC;  					- Data Input 
*    Frac16 f16UpperLimit;				- Data Input
*    Frac16 f16LowerLimit;              - Data Input  
*    Frac16 f16Delay_x1;				- Data Input/Output 
*    Frac16 f16Delay_x2;				- Data Input/Output 
*          
* @param    f16Error
*           defined as f16Error = f16Required - f16Actual
*
* @param    const pudtControlStruct
*           const pointer to PID controller data structure
*			1)  16-bit control output parameter of previous output -f16ControlOutN_1
*           2)  16-bit controller parameter KA related to error(n), 
*           3)  16-bit controller parameter KB related to error(n-1), 
*           4)  16-bit controller parameter KC related to error(n-2), 
*           5)  16-bit controller output upper limit
*           6)  16-bit controller output lower limit
*			7)  16-bit controller output Delay_x1;	
*           8)  16-bit controller output Delay_x2;	
*
* @return   Y0 - u(n) 
*           1) action value u(n) in range [-1,1]
*		
* @remarks 	
*           Difference equation:
*           y(n) = y(n-1) + KA*error(n) + KB*error(n-1) + KC*error(n-2) ;
*
*           y(n)        <-> controller output - action value
*           y(n-1)      <-> delayed action value u(n-1) (stored in 16-bit)
*           error(n)    <-> controller input (error)
*           error(n-1)  <-> controller input (error in n-1 step)
*           error(n-2)  <-> controller input (error in n-2 step)
*           KA          <-> 1st controller coefficient related to error(n)
*           KB          <-> 2nd controller coefficient related to error(n-1)
*           KC          <-> 3rd controller coefficient related to error(n-2)
*
*           Transition from the continuous to the discrete time domain (Discretize)
*
*     +-------------------+-------------------------------+----------------------------------------+
*     | Controller Coeff. | Integration w. Backward Rect. | Integration w. Bilinear Transformation |
*     |                   | Derivation  w. Backward Rect. | Derivation   w. Backward Rectangular   |
*     +-------------------+-------------------------------+----------------------------------------+ 
*     |       KA          |  Kp+Ki*Ts+Kd/Ts               |  Kp+Ki*Ts/2+Kd/Ts                      |
*     |       KB          | -Kp-2*Kd/Ts                   | -Kp+Ki*Ts/2-2*Kd/Ts                    |
*     |       KC          |  Kd/Ts                        |  Kd/Ts                                 |
*     +-------------------+-------------------------------+----------------------------------------+
*
*           Kp - proportional gain
*           Ki - integral gain
*           Kd - derivation gain
*           Ts - sampling period
*   
*           f16u(n) = f16u(n-1)+KA*f16Err(n)+KB*f16Err(n-1)+KC*f16Err(n-2);
*           
*       
*           f16u(n)         <-> fractional representation [-1,1] control output
*           f16Err(n)       <-> fractional representation [-1,1] control input
*           KA              <-> KA is fractional representation in range [-1,1]
*           KB              <-> KB is fractional representation in range [-1,1]
*           KC              <-> KC is fractional representation in range [-1,1]
*
*
****************************************************************************/ 

asm Frac16 PCLIB_ControllerPIDFAsm(
	Frac16 f16Error, 
	PCLIB_CONTROL_PID_PARAMS_T * const pudtControlStruct); 

/******************************************************************************
* Inline functions
******************************************************************************/

#endif /*__PCLIB_CONTROL_PID_ASM_H__*/
