/*******************************************************************************
* FILE NAME: etpu_sc.c              COPYRIGHT (c) Freescale Semiconductor 2004
*                                               All Rights Reserved
* DESCRIPTION:
* This file contains the ETPU Speed Controller(SC) API.
*===============================================================================
* REV      AUTHOR      DATE        DESCRIPTION OF CHANGE
* ---   -----------  ----------    ---------------------
* 1.0   A. Butok     26/Oct/04     Updated after merge of PID to SC,
*       M. Brejl                   ready for release.
*       M. Princ
* 2.0   M. Brejl     17/Dec/04     Updated for new build structure.
* 2.1   M. Brejl     18/Dec/04     fs_etpu_sc_get_configuration added.
* 2.2   M. Brejl     18/Feb/05     PWMM_chan init parameter replaced by
*                                  output_chan, output_offset and link_chan
*                                  parameters, in order to support update of
*                                  any function parameter, not only the PWMM
*                                  duty-cycle.
* 2.3   M. Brejl     21/Oct/05     Correction of fs_etpu_sc_set_integral_portion
* 2.4   M. Brejl     21/Jan/12     Resolver support added.
*******************************************************************************/
#include "etpu_util.h"          /* Utility routines for working with the eTPU */
#include "etpu_sc.h"            /* eTPU SC API */
#include "etpu_hd.h"            /* eTPU HD API */

extern uint32_t fs_etpu_data_ram_start;
extern uint32_t fs_etpu_data_ram_ext;

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_init
*PURPOSE      : This routine is used to initialize the eTPU channel for the SC
*               function.
*INPUTS NOTES : This function has 19 parameters:
*  channel            - This is the Speed Controller (SC) channel number.
*                       0-31 for ETPU_A and 64-95 for ETPU_B.
*  priority           - This is the priority to assign to the SC function.
*                       This parameter should be assigned a value of:
*                       FS_ETPU_PRIORITY_HIGH or
*                       FS_ETPU_PRIORITY_MIDDLE or
*                       FS_ETPU_PRIORITY_LOW or
*                       FS_ETPU_PRIORITY_DISABLED.
*  mode               - This is the function mode.
*                       This parameter should be assigned a value of:
*                       FS_ETPU_SC_MASTER or
*                       FS_ETPU_SC_SLAVE.
*  decoder_type       - This is the type of the decoder (QD or HD).
*                       This parameter should be assigned a value of:
*                       FS_ETPU_SC_QD or
*                       FS_ETPU_SC_HD_REV_PERIOD or
*                       FS_ETPU_SC_HD_SEC_PERIOD_1 or
*                       FS_ETPU_SC_HD_SEC_PERIOD_2 or
*                       FS_ETPU_SC_HD_SEC_PERIOD_3 or
*                       FS_ETPU_SC_HD_SEC_PERIOD_4 or
*                       FS_ETPU_SC_RSLV.
*  configuration      - This is the required configuration of SC.
*                       This parameter should be assigned a value of:
*                       FS_ETPU_SC_RAMP_OFF_PID_OFF or
*                       FS_ETPU_SC_RAMP_OFF_PID_ON or
*                       FS_ETPU_SC_RAMP_ON_PID_OFF or
*                       FS_ETPU_SC_RAMP_ON_PID_ON.
*  period             - This is the update period, as number of TCR1 clocks.
*                       This parameter applies in the Master
*                       Mode only (mode=FS_ETPU_SC_MASTER).
*  start_offset       - This parameter is used to synchronize various
*                       eTPU functions that generate a signal.
*                       The first SC update starts the start_offset TCR1
*                       clocks after initialization. This parameter applies
*                       in the Master Mode only (mode=FS_ETPU_SC_MASTER).
*  services_per_irq   - This parameter defines the number of updates after
*                       which an interrupt service request is generated
*                       to the CPU.
*  p_pid_params       - This is the pointer to a sc_pid_params_t structure.
*  p_ramp_params      - This is the pointer to a sc_ramp_params_t structure.
*  HD_QD_RSLV_chan    - This is the number of a channel the QD, HD or RSLV is
*                       assigned. 0-31 for ETPU_A and 64-95 for ETPU_B.
*  output_chan        - SC writes the PID output to a recipient function
*                       input parameter. This is the recipient function
*                       channel number.
*                       0-31 for ETPU_A and 64-95 for ETPU_B.
*  output_offset      - SC writes the PID output to a recipient function
*                       input parameter. This is the recipient function
*                       parameter offset.
*                       Function parameter offsets are defined in
*                       etpu_<func>_auto.h file.
*  link_chan          - This is the channel number of a channel which receives
*                       a link after SC updates output. If SC updates PWM
*                       duty-cycles it should be a PWMMDC channel.
*                       0-31 for ETPU_A and 64-95 for ETPU_B.
*  omega_max_rpm      - This is the maximum possible motor speed in [rpm].
*  omega_min_rpm      - This is the minimum possible motor speed in [rpm].
*                       This parameter applies only if Hall Decoder
*                       is used (decoder_type=FS_ETPU_SC_HD).
*  pole_pairs         - If Hall Decoder is used (decoder_type=FS_ETPU_SC_HD)
*                       this is the number of motor pole pairs.
*                       If Resolver is used (decoder_type=FS_ETPU_SC_RSLV)
*                       this is the number of resolver pole pairs.
*                       If Quadrature Decoder is used (decoder_type=FS_ETPU_QD)
*                       this parameter does not apply.
*  HD_QD_etpu_tcr_freq- This is the frequency of TCR clock used
*                       by HD or QD channel, in [Hz].
*                       If Resolver is used (decoder_type=FS_ETPU_SC_RSLV)
*                       this parameter does not apply.
*  rslv_freq          - If Resolver is used (decoder_type=FS_ETPU_SC_RSLV)
*                       This is the resolver update frequency in Hz.
*  qd_pc_per_rev      - This is the number of QD Position Counter increments
*                       per revolution.
*                       This parameter applies only if Quadrature Decoder
*                       is used (decoder_type=FS_ETPU_SC_QD).
*
*RETURNS NOTES: Error codes which can be returned are: FS_ETPU_ERROR_VALUE,
*               FS_ETPU_ERROR_MALLOC
*******************************************************************************/
int32_t fs_etpu_sc_init( uint8_t channel,
                         uint8_t priority,
                         uint8_t mode,
                         uint8_t decoder_type,
                         uint8_t configuration,
                        uint24_t period,
                        uint24_t start_offset,
                        uint24_t services_per_irq,
                sc_pid_params_t* p_pid_params,
               sc_ramp_params_t* p_ramp_params,
                         uint8_t HD_QD_RSLV_chan,
                         uint8_t output_chan,
                        uint16_t output_offset,
                         uint8_t link_chan,
                        uint24_t omega_max_rpm,
                        uint24_t omega_min_rpm,
                         uint8_t pole_pairs,
                        uint32_t HD_QD_etpu_tcr_freq,
                        uint32_t rslv_freq,
                        uint24_t qd_pc_per_rev )
{
   uint32_t * pba;
   uint32_t * pba_pid;
   uint8_t options;

   uint32_t hd_state_addr;
   uint32_t period_addr = 0;
   uint32_t dir_addr = 0;
   uint32_t pcsc_addr = 0;
   uint32_t tcr_addr = 0;
   uint32_t integacc1_addr = 0;
   uint32_t output_addr = 0;
   uint24_t scale = 0;
   uint24_t hd_max_period = 0;

   /***************************************
   * Parameters bounds check.
   ***************************************/
   #ifdef FS_ETPU_MC_PARAM_CHECK
   if(((channel>31)&&(channel<64))||(channel>95)||
      (priority>FS_ETPU_PRIORITY_HIGH)||
      (mode>FS_ETPU_SC_MASTER)||
      (decoder_type>FS_ETPU_SC_HD_SEC_PERIOD_4)||
      (configuration>FS_ETPU_SC_RAMP_ON_PID_ON)||
      (period>0x800000)||
      (start_offset>0x800000)||
      ((HD_QD_RSLV_chan>31)&&(HD_QD_RSLV_chan<64))||(HD_QD_RSLV_chan>95)||
      ((link_chan>31)&&(link_chan<64))||(link_chan>95)||
      ((output_chan>31)&&(output_chan<64))||(output_chan>95))
   {
      return(FS_ETPU_ERROR_VALUE);
   }
   #endif

   /***************************************
   * PRAM allocation.
   ***************************************/
   /* channel parameters allocation. */
   if ((pba=fs_etpu_malloc(FS_ETPU_SC_NUM_PARMS)) == 0)
   {
      return(FS_ETPU_ERROR_MALLOC);
   }
   /* PID structure allocation. */
   if ((pba_pid=fs_etpu_malloc(FS_ETPU_MC_CTRL_PID_STR_SIZE)) == 0)
   {
      return(FS_ETPU_ERROR_MALLOC);
   }

   /***************************************
   * Write chan config register.
   ***************************************/
   eTPU->CHAN[channel].CR.R = (FS_ETPU_SC_TABLE_SELECT << 24) +
                              (FS_ETPU_SC_FUNCTION_NUMBER << 16) +
                              (((uint32_t)pba - fs_etpu_data_ram_start) >> 3);
   eTPU->CHAN[channel].SCR.R = configuration;

   /***************************************
   * Write parameters.
   ***************************************/
   /* proportional and integral portions are always calculated */
   options = FS_ETPU_MC_CTRL_PID_P_GAIN | FS_ETPU_MC_CTRL_PID_I_GAIN;
   /* derivative portion is optional */
   if (p_pid_params->D_gain != 0)
      options |= FS_ETPU_MC_CTRL_PID_D_GAIN;

   /* calculate scale parameter and eTPU pointer addresses */
   if (decoder_type == FS_ETPU_SC_QD)
   {
      scale = (uint24_t)((((30*HD_QD_etpu_tcr_freq)/omega_max_rpm)<<8)/
                       qd_pc_per_rev);
      pcsc_addr = (uint32_t)((eTPU->CHAN[HD_QD_RSLV_chan].CR.B.CPBA << 3) + 
                             FS_ETPU_QD_PC_SC_OFFSET);
      tcr_addr = (uint32_t)((eTPU->CHAN[HD_QD_RSLV_chan].CR.B.CPBA << 3) + 
                             FS_ETPU_QD_LAST_EDGE_OFFSET);
   }
   else if (decoder_type == FS_ETPU_SC_RSLV)
   {
      scale = (uint24_t)(30*rslv_freq/(omega_max_rpm*pole_pairs));
      integacc1_addr = (uint32_t)((eTPU->CHAN[HD_QD_RSLV_chan].CR.B.CPBA << 3) + 
                             FS_ETPU_RSLV_INTEGACC1_OFFSET);
   }
   else
   {
      scale = (uint24_t)((60*HD_QD_etpu_tcr_freq)/
                       (decoder_type*omega_max_rpm*pole_pairs));
      hd_max_period = (uint24_t)((60*HD_QD_etpu_tcr_freq)/
                                 (decoder_type*omega_min_rpm*pole_pairs));
      hd_state_addr = fs_etpu_hd_state_struct_addr(HD_QD_RSLV_chan);
      dir_addr = hd_state_addr + FS_ETPU_HD_COMMONSTR_DIRECTION_OFFSET;
      if (decoder_type > FS_ETPU_SC_HD_REV_PERIOD)
         period_addr = hd_state_addr + FS_ETPU_HD_COMMONSTR_SECTORPERIOD_OFFSET;
      else
         period_addr = hd_state_addr + FS_ETPU_HD_COMMONSTR_REVPERIOD_OFFSET;
   }
   output_addr = (uint32_t)((eTPU->CHAN[output_chan].CR.B.CPBA << 3) + 
                            output_offset);

   /* write channel parameters */
   *(pba + ((FS_ETPU_SC_PERIOD_OFFSET - 1)>>2)) = period;
   *(pba + ((FS_ETPU_SC_STARTOFFSET_OFFSET - 1)>>2)) = start_offset;
   *(pba + ((FS_ETPU_SC_SERVICESPERIRQ_OFFSET - 1)>>2)) = services_per_irq;
   *((uint8_t*)pba + FS_ETPU_SC_LINKCHAN_OFFSET) = (uint8_t)(link_chan + 64);
   *(pba + ((FS_ETPU_SC_PPID_OFFSET - 1)>>2)) = ((uint32_t)pba_pid - 
                                                fs_etpu_data_ram_start)& 0x1FFF;
   *(pba + ((FS_ETPU_SC_POUTPUT_OFFSET - 1)>>2)) = output_addr & 0x1FFF;
   *(pba + ((FS_ETPU_SC_OMEGADESIRED_OFFSET - 1)>>2)) = 0;
   *(pba + ((FS_ETPU_SC_OMEGADESIREDRAMP_OFFSET - 1)>>2)) = 0;
   *(pba + ((FS_ETPU_SC_RAMPINCRUP_OFFSET - 1)>>2)) = 
                                          (uint32_t)p_ramp_params->ramp_incr_up;
   *(pba + ((FS_ETPU_SC_RAMPINCRDOWN_OFFSET - 1)>>2)) = 
                                        (uint32_t)p_ramp_params->ramp_incr_down;
   *(pba + ((FS_ETPU_SC_ERROR_OFFSET - 1)>>2)) = 0;
   *(pba + ((FS_ETPU_SC_OMEGAACTUAL_OFFSET - 1)>>2)) = 0;
   *(pba + ((FS_ETPU_SC_PHDPERIOD_OFFSET - 1)>>2)) = period_addr & 0x1FFF;
   *(pba + ((FS_ETPU_SC_PHDDIRECTION_OFFSET - 1)>>2)) = dir_addr & 0x1FFF;
   *(pba + ((FS_ETPU_SC_MAXHDPERIOD_OFFSET - 1)>>2)) = hd_max_period;
   *(pba + ((FS_ETPU_SC_SCALE_OFFSET - 1)>>2)) = scale;
   *(pba + ((FS_ETPU_SC_PQDPCSC_OFFSET - 1)>>2)) = pcsc_addr & 0x1FFF;
   *(pba + ((FS_ETPU_SC_PQDTCRVALUE_OFFSET - 1)>>2)) = tcr_addr & 0x1FFF;
   *(pba + ((FS_ETPU_SC_QDLASTTCR_OFFSET - 1)>>2)) = 0;
   *(pba + ((FS_ETPU_SC_PRSLVINTEGACC1_OFFSET - 1)>>2)) = integacc1_addr & 0x1FFF;

   /* write PID structure */
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_PGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->P_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->I_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_DGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->D_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_LIMITPOS_OFFSET - 1)>>2)) = 
                                      (uint32_t)p_pid_params->positive_limit>>1;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_LIMITNEG_OFFSET - 1)>>2)) = 
                                      (uint32_t)p_pid_params->negative_limit>>1;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IK1H_OFFSET - 1)>>2)) = 0;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IK1L_OFFSET - 1)>>2)) = 0;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_ERRORK1_OFFSET - 1)>>2)) = 0;
   *((uint8_t*)pba_pid + FS_ETPU_MC_CTRL_PID_OPTIONS_OFFSET) = options;
   *((uint8_t*)pba_pid + FS_ETPU_MC_CTRL_PID_FLAGS_OFFSET) = 0;

   /***************************************
   * Write HSR.
   ***************************************/
   if(mode==FS_ETPU_SC_MASTER)
   {
      if(decoder_type == FS_ETPU_SC_QD)
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_QD_MASTER;
      else if(decoder_type == FS_ETPU_SC_RSLV)
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_RSLV_MASTER;
      else
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_HD_MASTER;
   }
   else
   {
      if(decoder_type == FS_ETPU_SC_QD)
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_QD_SLAVE;
      else if(decoder_type == FS_ETPU_SC_RSLV)
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_RSLV_SLAVE;
      else
         eTPU->CHAN[channel].HSRR.R = FS_ETPU_SC_INIT_HD_SLAVE;
   }

   /****************************************
    * Set channel priority and enable.
    ***************************************/
   fs_etpu_enable(channel, priority);

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_update
*PURPOSE      : This function executes the SC update..
*INPUTS NOTES : This function has 1 parameter:
*   channel           - This is the SC channel number.
*                       This parameter must be assigned the same value
*                       as the channel parameter of the initialization
*                       function was assigned.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_update(uint8_t channel)
{
   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   fs_etpu_set_hsr(channel, FS_ETPU_SC_CALC);

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_set_configuration
*PURPOSE      : This function changes the SC configuration.
*INPUTS NOTES : This function has 2 parameters:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*   configuration - This is the required configuration of SC.
*                   This parameter should be assigned a value of:
*                   FS_ETPU_SC_RAMP_OFF_PID_OFF or
*                   FS_ETPU_SC_RAMP_OFF_PID_ON or
*                   FS_ETPU_SC_RAMP_ON_PID_OFF or
*                   FS_ETPU_SC_RAMP_ON_PID_ON.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_set_configuration(uint8_t  channel, uint8_t configuration)
{
   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95)||
           (configuration>FS_ETPU_SC_RAMP_ON_PID_ON))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   eTPU->CHAN[channel].SCR.R = configuration;

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_set_pid_params
*PURPOSE      : This function changes the PID parameter values.
*INPUTS NOTES : This function has 2 parameters:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*   p_pid_params  - This is the pointer to a sc_pid_params_t structure.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_set_pid_params(uint8_t  channel,
                          sc_pid_params_t* p_pid_params)
{
   uint32_t * pba_pid;
   uint8_t  options;

   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   /* get PID structure address */
   pba_pid = (uint32_t*)(fs_etpu_data_ram_start +
                    fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_PPID_OFFSET));

   /* proportional and integral portions are always calculated */
   options = FS_ETPU_MC_CTRL_PID_P_GAIN | FS_ETPU_MC_CTRL_PID_I_GAIN;
   /* derivative portion is optional */
   if (p_pid_params->D_gain != 0)
      options |= FS_ETPU_MC_CTRL_PID_D_GAIN;

   /* write PID structure */
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_PGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->P_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->I_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_DGAIN_OFFSET - 1)>>2)) = 
                                                 (uint32_t)p_pid_params->D_gain;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_LIMITPOS_OFFSET - 1)>>2)) = 
                                      (uint32_t)p_pid_params->positive_limit>>1;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_LIMITNEG_OFFSET - 1)>>2)) = 
                                      (uint32_t)p_pid_params->negative_limit>>1;
   *((uint8_t*)pba_pid + FS_ETPU_MC_CTRL_PID_OPTIONS_OFFSET) = options;

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_set_ramp_params
*PURPOSE      : This function changes the RAMP parameter values.
*INPUTS NOTES : This function has 2 parameters:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*   p_ramp_params - This is the pointer to a sc_ramp_params_t structure.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_set_ramp_params(uint8_t  channel,
                          sc_ramp_params_t* p_ramp_params)
{
   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   fs_etpu_set_chan_local_24(channel, FS_ETPU_SC_RAMPINCRUP_OFFSET,
                             (uint32_t)p_ramp_params->ramp_incr_up);
   fs_etpu_set_chan_local_24(channel, FS_ETPU_SC_RAMPINCRDOWN_OFFSET,
                             (uint32_t)p_ramp_params->ramp_incr_down);

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_set_omega_desired
*PURPOSE      : This function changes the desired value, as a portion
*               of the maximum value.
*INPUTS NOTES : This function has 2 parameters:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*   omega_desired - Desired input value, in range MIN24 to MAX24
*                   The sign determines the rotor direction.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_set_omega_desired(uint8_t   channel,
                                     fract24_t omega_desired)
{
   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   fs_etpu_set_chan_local_24(channel, FS_ETPU_SC_OMEGADESIRED_OFFSET,
                             (uint24_t)omega_desired);

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_set_integral_portion
*PURPOSE      : This function sets the integral portion.
*INPUTS NOTES : This function has 2 parameters:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*   i_k1          - This is the integral portion value.
*
*RETURNS NOTES: Error code that can be returned is: FS_ETPU_ERROR_VALUE.
*******************************************************************************/
int32_t fs_etpu_sc_set_integral_portion( uint8_t channel, fract24_t i_k1)
{
   fract24_t ik1h;
   fract24_t ik1l;
   uint32_t * pba_pid;

   /* Parameters bounds check */
   #ifdef FS_ETPU_MC_PARAM_CHECK
       if(((channel>31)&&(channel<64))||(channel>95))
       {
          return(FS_ETPU_ERROR_VALUE);
       }
   #endif

   if (i_k1 & 0x800000)
      i_k1 |= 0xFF000000;
   
   ik1h = i_k1 >> 9;
   ik1l = i_k1 << 15;

   /* get PID structure address */
   pba_pid = (uint32_t*)(fs_etpu_data_ram_start +
                    fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_PPID_OFFSET));

   /* write to PID structure */
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IK1H_OFFSET - 1)>>2)) = (uint24_t)ik1h;
   *(pba_pid + ((FS_ETPU_MC_CTRL_PID_IK1L_OFFSET - 1)>>2)) = (uint24_t)ik1l;

   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_omega_actual
*PURPOSE      : This function gets the actual angular value, as a portion
*               of the maximum value.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: omega_actual value.
*******************************************************************************/
fract24_t fs_etpu_sc_get_omega_actual( uint8_t channel)
{
   return(fs_etpu_get_chan_local_24s(channel,FS_ETPU_SC_OMEGAACTUAL_OFFSET));
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_saturation_flag
*PURPOSE      : This function returns the PID controller saturation flags.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: This function returns saturation flags. The returned value
*               can be one of:
*               FS_ETPU_SC_SATURATION_NO  (0) ... no saturation
*               FS_ETPU_SC_SATURATION_POS (1) ... saturation to positive limit
*               FS_ETPU_SC_SATURATION_NEG (2) ... saturation to negative limit
*******************************************************************************/
uint8_t fs_etpu_sc_get_saturation_flag(uint8_t channel)
{
   uint32_t * pba_pid;
   uint8_t flag;

   /* get PID structure address */
   pba_pid = (uint32_t*)(fs_etpu_data_ram_start +
                    fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_PPID_OFFSET));

   /* read from PID structure */
   flag = *((uint8_t*)pba_pid + FS_ETPU_MC_CTRL_PID_FLAGS_OFFSET);

   return(flag);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_ramp_output
*PURPOSE      : This function returns the RAMP output.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: ramp_output value
*******************************************************************************/
fract24_t fs_etpu_sc_get_ramp_output(uint8_t channel)
{
   return(fs_etpu_get_chan_local_24s(channel,
                                     FS_ETPU_SC_OMEGADESIREDRAMP_OFFSET));
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_output
*PURPOSE      : This function returns the PID controller output.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: PID controller output value
*******************************************************************************/
fract24_t fs_etpu_sc_get_output(uint8_t channel)
{
   uint32_t  output_addr;
   fract24_t output;

   output_addr = fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_POUTPUT_OFFSET);
   output = *((fract24_t *)(fs_etpu_data_ram_ext + output_addr-1));

   return(output);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_error
*PURPOSE      : This function returns the PID controller error.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: PID controller error value
*******************************************************************************/
fract24_t fs_etpu_sc_get_error(uint8_t channel)
{
   return(fs_etpu_get_chan_local_24s(channel, FS_ETPU_SC_ERROR_OFFSET));
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_integral_portion
*PURPOSE      : This function returns the PID controller integral portion.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: PID controller integral_portion value
*******************************************************************************/
fract24_t fs_etpu_sc_get_integral_portion(uint8_t channel)
{
   uint32_t * pba_pid;
   fract24_t ik1h;
   fract24_t ik1l;
   fract24_t ik1;

   /* get PID structure address */
   pba_pid = (uint32_t*)(fs_etpu_data_ram_start +
                    fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_PPID_OFFSET));

   /* read from PID structure */
   ik1h = *((fract24_t*)pba_pid + FS_ETPU_MC_CTRL_PID_IK1H_OFFSET-1);
   ik1l = *((fract24_t*)pba_pid + FS_ETPU_MC_CTRL_PID_IK1L_OFFSET-1);

   ik1 = (ik1h << 9) + (ik1l >> 15);

   return(ik1);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_configuration
*PURPOSE      : This function reads the current SC configuration.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: This function returms the configuration of SC.
*               The returns value may be one of:
*               FS_ETPU_SC_RAMP_OFF_PID_OFF or
*               FS_ETPU_SC_RAMP_OFF_PID_ON or
*               FS_ETPU_SC_RAMP_ON_PID_OFF or
*               FS_ETPU_SC_RAMP_ON_PID_ON.
*******************************************************************************/
uint8_t fs_etpu_sc_get_configuration(uint8_t  channel)
{
   return((uint8_t)(eTPU->CHAN[channel].SCR.R & 0x03));
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_pc_diff
*PURPOSE      : If speed is measured from QD (decoder_type = FS_ETPU_SC_QD)
*               this function gets the position difference between last two
*               updates, in QD position counts.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: This function returms the QD position difference as signed int24.
*******************************************************************************/
int24_t fs_etpu_sc_get_pc_diff(uint8_t channel)
{
	return(fs_etpu_get_chan_local_24s(channel, FS_ETPU_SC_QDPOSITIONDIFF_OFFSET));
}

/*******************************************************************************
*FUNCTION     : fs_etpu_sc_get_time_diff
*PURPOSE      : If speed is measured from QD (decoder_type = FS_ETPU_SC_QD)
*               this function gets the time difference between last two
*               updates, in TCR1 or TCR2 cycles.
*INPUTS NOTES : This function has 1 parameter:
*   channel       - This is the SC channel number.
*                   This parameter must be assigned the same value
*                   as the channel parameter of the initialization
*                   function was assigned.
*
*RETURNS NOTES: This function returms the QD time difference as unsigned int24.
*******************************************************************************/
uint24_t fs_etpu_sc_get_time_diff(uint8_t channel)
{
	return(fs_etpu_get_chan_local_24(channel, FS_ETPU_SC_QDTIMEDIFF_OFFSET));
}


/*********************************************************************
 *
 * Copyright:
 *	Freescale Semiconductor, INC. All Rights Reserved.
 *  You are hereby granted a copyright license to use, modify, and
 *  distribute the SOFTWARE so long as this entire notice is
 *  retained without alteration in any modified and/or redistributed
 *  versions, and that such modified versions are clearly identified
 *  as such. No licenses are granted by implication, estoppel or
 *  otherwise under any patents or trademarks of Freescale
 *  Semiconductor, Inc. This software is provided on an "AS IS"
 *  basis and without warranty.
 *
 *  To the maximum extent permitted by applicable law, Freescale
 *  Semiconductor DISCLAIMS ALL WARRANTIES WHETHER EXPRESS OR IMPLIED,
 *  INCLUDING IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A
 *  PARTICULAR PURPOSE AND ANY WARRANTY AGAINST INFRINGEMENT WITH
 *  REGARD TO THE SOFTWARE (INCLUDING ANY MODIFIED VERSIONS THEREOF)
 *  AND ANY ACCOMPANYING WRITTEN MATERIALS.
 *
 *  To the maximum extent permitted by applicable law, IN NO EVENT
 *  SHALL Freescale Semiconductor BE LIABLE FOR ANY DAMAGES WHATSOEVER
 *  (INCLUDING WITHOUT LIMITATION, DAMAGES FOR LOSS OF BUSINESS PROFITS,
 *  BUSINESS INTERRUPTION, LOSS OF BUSINESS INFORMATION, OR OTHER
 *  PECUNIARY LOSS) ARISING OF THE USE OR INABILITY TO USE THE SOFTWARE.
 *
 *  Freescale Semiconductor assumes no responsibility for the
 *  maintenance and support of this software
 ********************************************************************/
