/******************** (C) COPYRIGHT 2008 STMicroelectronics ********************
* File Name          : PID_stm32.s
* Author             : MCD Application Team
* Version            : V1.0.1
* Date               : 10/20/2008
* Description        : This source file contains assembly optimized source code
*                      of a PID controller.
********************************************************************************
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/

 SECTION .text:CODE(2)

  PUBLIC PID_stm32
  IMPORT IntTerm
  IMPORT PrevError

#define Err     R0    /* 1st function input: Error  */
#define Coeff   R1    /* 2nd fct input: Address of coefficient table */
#define Kd      R1
#define Ki      R2
#define Kp      R3

#define Out     R4
#define Result  R2
#define Integ   R5
#define PrevErr R12


/*******************************************************************************
* Function Name  : DoPID
* Description    : PID in ASM, Error computed outside the routine
* Input          : Error: difference between reference and measured value
*                  Coeff: pointer to the coefficient table
* Output         : None
* Return         : PID output (command)
*******************************************************************************/
PID_stm32:

  PUSH {R4, R5, R9}

  LDR R12, =IntTerm
  LDR R9, =PrevError

  LDRH Kp, [Coeff, #0]  /* Load Kp */
  LDRH Ki, [Coeff, #2]  /* Load Ki */
  LDRH Kd, [Coeff, #4]  /* Load Kd and destroy Coeff*/
  LDRH Integ, [R12, #0]  /* Last Integral Term */
  LDRH PrevErr, [R9, #0]  /* Previous Error */

  MLA Integ, Ki, Err, Integ   /* IntTerm += Ki*error */
  MLA Out, Kp, Err, Integ      /* Output = (Kp * error) + InTerm */
  SUBS PrevErr, Err, PrevErr    /* PrevErr now holds DeltaError = Error - PrevError */
  MLA Result, Kd, PrevErr, Out  /* Output += Kd * DeltaError */

  LDR R12, =IntTerm
  STRH Integ, [R12, #0]     /* Write back InTerm */
  STRH Err, [R9, #0]        /* Write back PrevError */

  MOV R0, Result
  UXTH R0, R0
  POP {R4, R5, R9}
  BX LR

  END

/******************* (C) COPYRIGHT 2008 STMicroelectronics *****END OF FILE****/