pid_source.c

this is matlab pid tuning code

// Two 16-bits fixed-point PID implementations in C with 
// the Legacy code tool.
//
// Usage :
// PID function definitions. 
//
// Author :
// Gilles Bailly, nov. 2008.
// LCAR UMR5589-CNRS, Toulouse, France.



#include "pid_header.h"

//*************************************************************************
// 16-bits fixed-point PID controller in single precsion
// Data and pid coefficients are coded in 16-bits fixed-point
// format.
// Data values cover 10-bits range and pid coefs 16-bits. After
// each 16x16 bits multiplications, downcasts are performed and the 
// internal state is then stored in single precision (16-bits).
//*************************************************************************
int16_t pid_sp(
int16_t yr,
int16_t ym,
int16_t kp,
int16_t ki,
int16_t rounding_enable,
int16_t *pid_state){
    // Note that pid_state is int16_t pointer
    
    
    //*************************************
    // SIGNALS DECLARATIONS
    //*************************************
    // command signal
    int16_t uc; 
    // error signal
    int16_t z;    
    // working 32-bits register
    int32_t tmp32;    
    
    //*************************************
    // ERROR SIGNAL
    //*************************************
    // z = yr - ym
    z = yr - ym;
    
    //*************************************
    // STATE SPACE OUTPUT EQUATION
    //*************************************
    // u = 1*x + kp*z
    // u = 1*x + up
    tmp32 = (int32_t)(z * kp);
    if(rounding_enable>0){
       tmp32 += DATA_HALF_LSB; 
    }
    tmp32 = tmp32>>COEFF_FRAC_PART;
    uc = *pid_state + (int16_t)tmp32;
    
    //*************************************
    // STATE UPDATE EQUATION
    //*************************************
    // qx = 1*x + ki*z 
    // qx = 1*x + ui
    tmp32 = (int32_t)(z * ki);
    if(rounding_enable>0){
       tmp32 += DATA_HALF_LSB; 
    }
    tmp32 = tmp32>>COEFF_FRAC_PART;
    *pid_state = *pid_state + (int16_t)tmp32;
    
    //*************************************
    // OUTPUT COMMAND SIGNAL
    //*************************************
    return uc;
}

//*************************************************************************
// 16-bits fixed-point PID controller in double precsion
// Data and pid coefficients are coded in 16-bits fixed-point
// format.
// Data values cover 10-bits range and pid coefs 16-bits. After
// each 16x16 bits multiplications,  then all following operations are
// are performed  in double precision (32-bits). The internal state is also
// stored in double precision. The required downcast is just done after 
// the last operation which is the command signal sum.
//*************************************************************************

int16_t pid_dp(
int16_t yr,
int16_t ym,
int16_t kp,
int16_t ki,
int16_t rounding_enable,
int32_t *pid_state){ 
    // Note that pid_state is int32_t pointer
    
    //*************************************
    // SIGNALS DECLARATIONS
    //*************************************
    // command signal
    int16_t uc; 
    // error signal
    int16_t z;    
    // working 32-bits register
    int32_t tmp32;

    //*************************************
    // ERROR SIGNAL
    //*************************************
    // z = yr - ym 
    z = yr - ym;

    //*************************************
    // STATE SPACE OUTPUT EQUATION
    //*************************************
    // u = 1*x + kp*z 
    // u = 1*x + up
    tmp32 = (int32_t)(z * kp);
    tmp32 = *pid_state + tmp32;
    if(rounding_enable>0){
       tmp32 += DATA_HALF_LSB; 
    }
    uc = (int16_t)(tmp32>>COEFF_FRAC_PART);

    //*************************************
    // STATE UPDATE EQUATION
    //*************************************
    // qx = 1*x + ki*z 
    // qx = 1*x + ui
    tmp32 = (int32_t)(z * ki);
    *pid_state = *pid_state + tmp32;
    
    //*************************************
    // OUTPUT COMMAND SIGNAL
    //*************************************
    return uc;
}