/*******************************************************************************
* FILE NAME: etpu_app_pmsmrsvc.c      COPYRIGHT (c) Freescale Semiconductor 2012
*                                               All Rights Reserved     
*
* DESCRIPTION:                                                          
* This file contains the eTPU application
* PMSM with Resolver, Speed Vector Control API.
================================================================================
* REV      AUTHOR      DATE        DESCRIPTION OF CHANGE                 
* ---   -----------  ----------    ---------------------                 
* 1.0   M. Brejl     25/Apr/12     Initial version.  
*******************************************************************************/

/* general routines */
#include "etpu_util.h"             /* Utility routines for working eTPU */

/* eTPU Application PMSMRSVC API */
#include "etpu_app_pmsmrsvc.h"

/* eTPU Function APIs */
#include "etpu_pwmmac.h" /* PWMMAC */
#include "etpu_rslv.h"   /* RSLV */
#include "etpu_sc.h"     /* SC */
#include "etpu_bc.h"     /* BC */
#include "etpu_pmsmvc.h" /* PMSMVC */
#include "etpu_asac.h"   /* ASAC */

extern uint32_t etpu_a_tcr1_freq;
extern uint32_t etpu_a_tcr2_freq;
extern uint32_t etpu_b_tcr1_freq;
extern uint32_t etpu_b_tcr2_freq;

/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_init
*PURPOSE      : To initialize eTPU to implement PMSM with Quadrature Encoder, 
*               Speed Vector Control application.
*INPUTS NOTES : This function has the following parameters:
* pmsmrsvc_instance  - This is a pointer to pmsmrsvc_instance_t structure, 
*                       which is filled by fs_etpu_app_pmsmrsvc_init. 
*                       This structure must be declared in the user application. 
*                       When there are more instances of the application running
*                       simultaneously, there must be a separate 
*                       pmsmrsvc_instance_t structure for each one.
* PWM_master_channel  - This is the PWM master channel number. 
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
* PWM_phaseA_channel  - This is the PWM phase A channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
*                       In the case of complementary signal generation 
*                       (PWM_phases_type==FS_ETPU_APP_PMSMRSVC_COMPL_PAIRS), 
*                       the complementary channel is one channel higher.
* PWM_phaseB_channel  - This is the PWM phase B channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
*                       In the case of complementary signal generation 
*                       (PWM_phases_type==FS_ETPU_APP_PMSMRSVC_COMPL_PAIRS), 
*                       the complementary channel is one channel higher.
* PWM_phaseC_channel  - This is the PWM phase C channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
*                       In the case of complementary signal generation 
*                       (PWM_phases_type==FS_ETPU_APP_PMSMRSVC_COMPL_PAIRS), 
*                       the complementary channel is one channel higher.
* RSLV_channel        - This is the Resolver (excitation output) channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
* SC_channel          - This is the speed controller channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
* BC_channel          - This is the break controller channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
* PMSMVC_channel      - This is the PMSM vector control function channel number.
*                       0-31 for ETPU_A, and 64-95 for ETPU_B.
* ASAC_channel        - This is the analog sensing for AC motors (ASAC) channel 
*                       number. 0-31 for ETPU_A, and 64-95 for ETPU_B.
* PWM_phases_type     - This parameter determines the type of all PWM phases. 
*                       This parameter should be assigned a value of: 
*                       FS_ETPU_APP_PMSMRSVC_SINGLE_CHANNELS, or 
*                       FS_ETPU_APP_PMSMRSVC_COMPL_PAIRS.
* PWM_freq_hz         - This is the PWM frequency in Hz.
* PWM_dead_time_ns    - This is the PWM dead-time in ns.
* speed_range_rpm     - This is the maximum motor speed in rpm.
* speed_min_rpm       - This is the minimum (measurable) motor speed in rpm.
* dc_bus_voltage_range_mv - This is the maximum measurable DC-bus voltage in mV.
* motor_pole_pairs    - This is the number of motor pole-pairs.
* rslv_pole_pairs     - This is the number of resolver pole-pairs.
* SC_freq_hz          - This is the speed controller update frequency in Hz. 
*                       The assigned value must be equal to the PWM_freq_hz 
*                       divided by 1, 2, 3, 4, 5, ...
* SC_PID_gain_permil    - This is the speed PI controller gain in millesimals.
* SC_I_time_constant_us - This is the speed PI controller integral time constant 
*                         in µs.
* SC_ramp_time_ms     - This parameter defines the required speed ramp time 
*                       in ms. A step change of the required speed from 0 to 
*                       speed_range_rpm is slowed down by the ramp to take 
*                       the defined time.
* PMSMVC_D_PID_gain_permil    - This is the D-current (flux controlling current)
*                               PI controller gain in millesimals.
* PMSMVC_D_I_time_constant_us - This is the D-current (flux controlling current)
*                               PI controller integral time constant in µs.
* PMSMVC_Q_PID_gain_permil    - This is the Q-current (torque controlling) 
*                               PI controller gain in millesimals.
* PMSMVC_Q_I_time_constant_us - This is the Q-current (torque controlling) 
*                               PI controller integral time constant in µs.
* PMSM_Ke_mv_per_krpm - This is the motor electrical constant in mV/1000RPM.
* PMSM_L_uH           - This is the motor induction in µH.
* phase current_range_ma - This is the maximum measurable phase current in mA.
* BC_mode             - This is the function mode. This parameter should be 
*                       assigned a value of: 
*                       FS_ETPU_APP_PMSMRSVC_BC_MODE_ON_OFF, or 
*                       FS_ETPU_APP_PMSMRSVC_BC_MODE_PWM.
* BC_polarity         - This is the BC output polarity. This parameter should be
*                       assigned a value of: 
*                       FS_ETPU_APP_PMSMRSVC_BC_ON_HIGH, or 
*                       FS_ETPU_APP_PMSMRSVC_BC_ON_LOW.
* BC_u_dc_bus_ON_perc - This is the proportion between U_DC_BUS, above which the 
*                       BC output is ON, and the nominal U_DC_BUS, expressed 
*                       in percentage (usually about 130%).
* BC_u_dc_bus_OFF_perc- This is the proportion between U_DC_BUS, below which 
*                       the BC output is OFF, and the nominal U_DC_BUS, 
*                       expressed in percentage (usually about 110%).
* ASAC_polarity       - This is the polarity to assign to the ASAC function. 
*                       This parameter should be assigned a value of: 
*                       FS_ETPU_APP_PMSMRSVC_ASAC_PULSE_HIGH or
*                       FS_ETPU_APP_PMSMRSVC_ASAC_PULSE_LOW.
* ASAC_measure_time_us- Time from the first (triggering) edge to the second edge
*                       at which the result queue is supposed to be ready in the
*                       DATA_RAM (in us). This value depends on the A/D 
*                       conversion time and DMA transfer time.
* ASAC_result_queue   - Pointer to the result queue in eTPU DATA RAM. Result 
*                       queue is an array of 16-bit words that contains the 
*                       measured values.
* ASAC_bit_shift      - This parameter defines how to align data from the result
*                       queue into fract24 (or int24). This parameter should be 
*                       assigned a values of: 
*                       FS_ETPU_APP_PMSMRSVC_ASAC_SHIFT_LEFT_BY_8,
*                       FS_ETPU_APP_PMSMRSVC_ASAC_SHIFT_LEFT_BY_10,
*                       FS_ETPU_APP_PMSMRSVC_ASAC_SHIFT_LEFT_BY_12, or
*                       FS_ETPU_APP_PMSMRSVC_ASAC_SHIFT_LEFT_BY_16.
* ASAC_filter_time_constant_i_us - This is the time constant of an
*                                  Exponentially-Weighted Moving Average (EWMA) 
*                                  filter which applies when processing the 
*                                  phase current samples, in us.
* ASAC_filter_time_constant_u_us - This is the time constant of an EWMA filter 
*                                  which applies when processing the DC-bus 
*                                  voltage samples, in us.
*               
* RETURNS NOTES: This funstion returns 0 in case of no error.
*                Error codes that can be returned are: FS_ETPU_ERROR_MALLOC.
*
* PROJECT NOTES: Analog Sensing for AC Motors (ASAC) function is configured 
*                to process phase currents and DC-Bus current analog sample 
*                values available at eTPU DATA RAM address 
*                %valueof(ASAC_result_queue)% (value of the ASAC_result_queue 
*                parameter). An AD converter, triggered by ASAC output signal, 
*                must sample the analog signal, and the converted value must 
*                be written by DMA to address %valueof(ASAC_result_queue)%. <br>
*                <font color=red>The configuration of DMA channels and other 
*                necessary peripherals must be performed separately from eTPU 
*                configuration!</font>
*******************************************************************************/
int32_t fs_etpu_app_pmsmrsvc_init( pmsmrsvc_instance_t * pmsmrsvc_instance,
                                               uint8_t   PWM_master_channel,
                                               uint8_t   PWM_phaseA_channel,
                                               uint8_t   PWM_phaseB_channel,
                                               uint8_t   PWM_phaseC_channel,
                                               uint8_t   RSLV_channel,
                                               uint8_t   SC_channel,
                                               uint8_t   BC_channel,
                                               uint8_t   PMSMVC_channel,
                                               uint8_t   ASAC_channel,
                                               uint8_t   PWM_phases_type,
                                               uint32_t  PWM_freq_hz,
                                               uint32_t  PWM_dead_time_ns,
                                               int32_t   speed_range_rpm,
                                               int32_t   speed_min_rpm,
                                               int32_t   dc_bus_voltage_range_mv,
                                               uint8_t   motor_pole_pairs,
                                               uint8_t   rslv_pole_pairs,
                                               uint32_t  SC_freq_hz,
                                               int32_t   SC_PID_gain_permil,
                                               int32_t   SC_I_time_const_us,
                                               uint32_t  SC_ramp_time_ms,
                                               int32_t   PMSMVC_D_PID_gain_permil,
                                               int32_t   PMSMVC_D_I_time_const_us,
                                               int32_t   PMSMVC_Q_PID_gain_permil,
                                               int32_t   PMSMVC_Q_I_time_const_us,
                                               int32_t   PMSM_Ke_mv_per_krpm,
                                               int32_t   PMSM_L_uH,
                                               int32_t   phase_current_range_ma,
                                               uint8_t   BC_mode,
                                               uint8_t   BC_polarity,
                                               uint8_t   BC_u_dc_bus_ON_perc,
                                               uint8_t   BC_u_dc_bus_OFF_perc,
                                               uint8_t   ASAC_polarity,
                                               uint32_t  ASAC_measure_time_us,
                                               uint32_t *ASAC_result_queue,
                                               uint8_t   ASAC_bit_shift,
                                               uint32_t  ASAC_filter_time_constant_i_us,
                                               uint32_t  ASAC_filter_time_constant_u_us)
{
   sc_pid_params_t        sc_pid_params;
   sc_ramp_params_t       sc_ramp_params;
   pmsmvc_pid_params_t    pmsmvc_pid_d_params;
   pmsmvc_pid_params_t    pmsmvc_pid_q_params;
   pmsmvc_motor_params_t  pmsmvc_motor_params;

   fract24_t ASAC_forget_factor_i;
   fract24_t ASAC_forget_factor_u;
   int24_t ASAC_edge_offset;
   int32_t A,B;
   etpu_asac_eQADC_cmds_t eQADC_cmds_current_measurements;
   uint32_t eQADC_cmds_other_measurements[7];
   
   uint32_t etpu_tcr1_freq;
   uint32_t etpu_tcr2_freq;
   uint32_t PWM_period;
   uint32_t RSLV_shift;

   int32_t err_code;

/*******************************************************************************
 * 0) Get current eTPU engine TCR frequencies
 ******************************************************************************/
   if (PWM_master_channel<64)
   {
      etpu_tcr1_freq = etpu_a_tcr1_freq;
      etpu_tcr2_freq = etpu_a_tcr2_freq;
   }
   else
   {
      etpu_tcr1_freq = etpu_b_tcr1_freq;
      etpu_tcr2_freq = etpu_b_tcr2_freq;
   }

/*******************************************************************************
 * 
 * 1) Initialize PWM generator
 *
 *******************************************************************************
 /******************************************************************************
 * 1.0) Timing
 ******************************************************************************/
   PWM_period = etpu_tcr1_freq/PWM_freq_hz;
 /******************************************************************************
 * 1.1) Initialize PWMMAC channels
 *******************************************************************************
 * The PWM generator is configured to generate (either base channels only, or
 * base and complementary channel pairs) according to the following example: 
 * 
 *  PWM periods              |<--------------------->|<--------------------->|
 *                               
 *  Phase A with positive duty-cycle:
 *                                 _____________           _____________
 *    base channel           _____|             |_________|             |_____
 *                           ____                 _______                 ____
 *    complementary ch.          |_______________|       |_______________|
 *                               
 *  Phase B with positive duty-cycle:
 *                                    _______                 _______
 *    base channel           ________|       |_______________|       |________
 *                           _______           _____________           _______
 *    complementary ch.             |_________|             |_________|
 *                               
 *  Phase C with positive duty-cycle:
 *                                      ___                     ___
 *    base channel           __________|   |___________________|   |__________
 *                           _________       _________________       _________
 *    complementary ch.               |_____|                 |_____|
 *                               
 * For more information about PWM generation options refer to PWMMAC 
 * eTPU function documentation.
 ******************************************************************************/
   err_code = fs_etpu_pwmmac_init_3ph(
      PWM_master_channel,	/* master_channel */
      FS_ETPU_PRIORITY_MIDDLE,	/* priority */
      PWM_phaseA_channel,	/* phaseA_channel */
      FS_ETPU_PWMMAC_DUTY_POS,	/* phaseA_negate_duty */
      PWM_phaseB_channel,	/* phaseB_channel */
      FS_ETPU_PWMMAC_DUTY_POS,	/* phaseB_negate_duty */
      PWM_phaseC_channel,	/* phaseC_channel */
      FS_ETPU_PWMMAC_DUTY_POS,	/* phaseA_negate_duty */
      FS_ETPU_PWMMAC_MOD_SVM_STD,	/* modulation */
      0,	/* p_table */
      FS_ETPU_PWMMAC_NORMAL,	/* update */
      FS_ETPU_PWMMAC_CENTER_ALIGNED,	/* alignment */
      PWM_phases_type,	/* phases_type */
      FS_ETPU_PWMMAC_NO_SWAP,	/* swap */
      FS_ETPU_PWMMAC_PIN_HIGH,	/* base_ch_disable_pin_state */
      FS_ETPU_PWMMAC_PIN_HIGH,	/* compl_ch_disable_pin_state */
      10000,	/* start_offset */
      PWM_period,	/* period */
      800,	/* update_time */
      etpu_tcr1_freq/1000*PWM_dead_time_ns/1000000, /* dead_time */
      etpu_tcr1_freq/1000*PWM_dead_time_ns/1000000	/* min_pw */
   );
   if (err_code != 0)
  	  return (err_code);
 
/*******************************************************************************
 * 
 * 2) Initialize Resolver
 *
 *******************************************************************************
 * Angle Tracking Observer parameters (see AN3943, chapter 3.2.2)
 *
 * natural frequency: omega [rad/sec]
 * damping factor:    zeta  [-]
 * 
 * K1d = 1/pi * omega^2 * 1/fs^2
 * K2d = 2*zeta / (omega * 1/fs)
 *
 ******************************************************************************/
   /* 15us from RSLV PWM to RSLV sin/cos max */
   RSLV_shift = 15*etpu_tcr1_freq/1000000;

   err_code = fs_etpu_rslv_init(RSLV_channel,	/* engine: A; channel: 0 */
                                FS_ETPU_PRIORITY_MIDDLE,	/* priority: Middle */
                                FS_ETPU_RSLV_MODE_MASTER | FS_ETPU_RSLV_MODE_ONREQUEST 
                                | FS_ETPU_RSLV_MODE_LINK | FS_ETPU_RSLV_STATE_DIGNOSTICS_OFF,	/* mode */
                                PWM_period,	/* period */
                                PWM_period/2,	/* duty */
                                1000,	/* delay */
                                10000 - RSLV_shift, 	/* start_offset */
                                0,	/* sample_delay */
                                0x1000,	/* gain_sin */
                                0x1000,	/* gain_cos */
                                0x030000, /* dc_offset_sin */
                                0x040000, /* dc_offset_cos */
                                PMSMVC_channel,	/* link_chan */
                                0,	/* rate_irq */
                                0,	/* rate_link */
                                0.58670878 * 0x800000,	/* k1_d */
                                5,	/* k1_scale */
                                0.6484375 * 0x800000,	/* k2_d */
                                4);	/* k2_scale */
   if (err_code != 0)
  	  return (err_code);
   
/*******************************************************************************
 * 
 * 3) Initialize Speed Controller
 *
 ******************************************************************************/
/*******************************************************************************
 * 3.1) Define Speed Controller PID Parameters
 *******************************************************************************
 *  The P-gain and I-gain are calculated from the controller gain and
 *  integral time constant, given by parameters, and transformed to 24-bit
 *  fractional format 9.15:
 *     P_gain = PID_gain_permil/1000 * 0x008000;
 *     I_gain = PID_gain_permil/1000 * 1/update_freq_hz * 
 *              * 1000000/I_time_const_us * 0x008000;
 *  The D-gain is set to zero in order to have a PI-type controller. 
 *  The positive and negative limits, which are set in 16-bit fractional 
 *  format (1.15), can be adjusted in order to limit the speed controller
 *  output range, and also the integral portion range. 
 ******************************************************************************/
   sc_pid_params.P_gain = SC_PID_gain_permil*0x001000/125;
   sc_pid_params.I_gain = 0x008000*1000/SC_freq_hz*SC_PID_gain_permil/
                          SC_I_time_const_us;
   sc_pid_params.D_gain = 0;
   sc_pid_params.positive_limit = (int16_t)0x7FFF;
   sc_pid_params.negative_limit = (int16_t)0x8000;
/*******************************************************************************
 * 3.2) Define Speed Controller Ramp Parameters
 ******************************************************************************/
   sc_ramp_params.ramp_incr_up   = (0x7FFFFF/SC_freq_hz)*1000/SC_ramp_time_ms;
   sc_ramp_params.ramp_incr_down = (0x7FFFFF/SC_freq_hz)*1000/SC_ramp_time_ms;
/*******************************************************************************
 * 3.3) Initialize SC channel
 ******************************************************************************/
   err_code = fs_etpu_sc_init(
      SC_channel,	/* channel */
      FS_ETPU_PRIORITY_LOW,	/* priority */
      FS_ETPU_SC_SLAVE,	/* mode */
      FS_ETPU_SC_RSLV,	/* decoder_type */
      FS_ETPU_SC_RAMP_OFF_PID_OFF,	/* configuration */
      0,	/* period */
      0,	/* start_offset */
      SC_freq_hz/1000,	/* services_per_irq: get 1kHz interrupt */
      &sc_pid_params,	/* p_pid_params */
      &sc_ramp_params,	/* p_ramp_params */
      RSLV_channel,	/* HD_QD_RSLV_chan */
      PMSMVC_channel,	/* output_chan - NOT INITIALIZED YET */
      FS_ETPU_PMSMVC_IDQDESIRED_OFFSET + 4,	/* output_offset */
      BC_channel,	/* link_chan */
      speed_range_rpm,	/* omega_max_rpm */
      speed_min_rpm,	/* omega_min_rpm */
      rslv_pole_pairs,	/* pole_pairs */
      etpu_tcr1_freq,	/* HD_QD_etpu_tcr_freq */
      PWM_freq_hz, /* rslv_freq */
      0	/* qd_pc_per_rev */
   );
   if (err_code != 0)
  	  return (err_code);

/*******************************************************************************
 * 
 * 4) Initialize PMSM Vector Control Loop
 *
 ******************************************************************************/
/*******************************************************************************
 * 4.1) Define D-Current Controller PID Parameters
 *******************************************************************************
 *  The P-gain and I-gain are calculated from the controller gain and
 *  integral time constant, given by parameters, and transformed to 24-bit
 *  fractional format 9.15:
 *     P_gain = PID_gain_permil/1000 * 0x008000;
 *     I_gain = PID_gain_permil/1000 * 1/update_freq_hz * 
 *              * 1000000/I_time_const_us * 0x008000;
 *  The D-gain is set to zero in order to have a PI-type controller. 
 *  The positive and negative limits, which are set in 16-bit fractional 
 *  format (1.15), can be adjusted in order to limit the speed controller
 *  output range, and also the integral portion range. 
 ******************************************************************************/
   pmsmvc_pid_d_params.P_gain = PMSMVC_D_PID_gain_permil*0x001000/125;
   pmsmvc_pid_d_params.I_gain = 0x008000*1000/PWM_freq_hz*PMSMVC_D_PID_gain_permil
                                /PMSMVC_D_I_time_const_us;
   pmsmvc_pid_d_params.D_gain = 0;
   pmsmvc_pid_d_params.positive_limit = (int16_t)0x7FFF;
   pmsmvc_pid_d_params.negative_limit = (int16_t)0x8000;
/*******************************************************************************
 * 4.2) Define Q-Current Controller PID Parameters
 ******************************************************************************/
   pmsmvc_pid_q_params.P_gain = PMSMVC_Q_PID_gain_permil*0x001000/125;
   pmsmvc_pid_q_params.I_gain = 0x008000*1000/PWM_freq_hz*PMSMVC_Q_PID_gain_permil
                                /PMSMVC_Q_I_time_const_us;
   pmsmvc_pid_q_params.D_gain = 0;
   pmsmvc_pid_q_params.positive_limit = (int16_t)0x7FFF;
   pmsmvc_pid_q_params.negative_limit = (int16_t)0x8000;
/*******************************************************************************
 * 4.3) Define Motor Parameters
 *******************************************************************************
 ******************************************************************************/
   pmsmvc_motor_params.Ke = (long long)motor_pole_pairs*PMSM_Ke_mv_per_krpm
                           *speed_range_rpm*TWO_PI_0X200000
                           /(dc_bus_voltage_range_mv*1000);
   pmsmvc_motor_params.Ld = (long long)motor_pole_pairs*PMSM_L_uH*speed_range_rpm
                           *phase_current_range_ma*TWO_PI_0X200000
                           /( dc_bus_voltage_range_mv*60000000);
   pmsmvc_motor_params.Lq = pmsmvc_motor_params.Ld;
/*******************************************************************************
 * 4.4) Initialize PMSMVC channel
 ******************************************************************************/
   err_code = fs_etpu_pmsmvc_init( 
      PMSMVC_channel,	/* channel */
      FS_ETPU_PRIORITY_LOW,	/* priority */
      FS_ETPU_PMSMVC_SLAVE,	/* mode */
      FS_ETPU_PMSMVC_CIRCLE_LIMITATION_ON, /* circle_limitation_config */
      0, /* period */
      0, /* start_offset */
      0, /* services_per_irq */
      SC_channel, /* SC_chan */
      RSLV_channel, /* QD_RSLV_chan */
      0, /* qd_pc_per_rev */
      rslv_pole_pairs, /* rslv_pole_pairs */
      motor_pole_pairs, /* motor_pole_pairs */
      &pmsmvc_motor_params, /* p_motor_params */
      &pmsmvc_pid_d_params, /* p_pid_d_params */
      &pmsmvc_pid_q_params, /* p_pid_q_params */
      FS_ETPU_PMSMVC_INVMODINDEX_SVM, /* inv_mod_index */
      PWM_master_channel, /* output_chan */
      FS_ETPU_PWMMAC_INPUTS_OFFSET,	/* output_offset */
      PWM_master_channel,	/* link_chan */
      (long long)0x800000*PWM_period*speed_range_rpm/(20*etpu_tcr1_freq)  /* compensation_delay */
   );
   if (err_code != 0)
  	  return (err_code);
   
/*******************************************************************************
 * 4.5) Re-Initialize SC Output
 ******************************************************************************/
   fs_etpu_set_chan_local_24(SC_channel, FS_ETPU_SC_POUTPUT_OFFSET, 
                             (uint32_t)((eTPU->CHAN[PMSMVC_channel].CR.B.CPBA<<3) 
                             + (FS_ETPU_PMSMVC_IDQDESIRED_OFFSET + 4)));

/*******************************************************************************
 * 
 * 5) Initialize Break Controller
 *
 ******************************************************************************/
   err_code = fs_etpu_bc_init(
      BC_channel,	/* channel */
      FS_ETPU_PRIORITY_LOW,	/* priority */
      BC_mode,	/* mode */
      BC_polarity,	/* polarity */
      2*(etpu_tcr1_freq/PWM_freq_hz), /* period */
      0, /* start_offset */
      0, /* services_per_irq */
      0xFFFFFE, /* u_dc_bus_ON */
      0xFFFFFF  /* u_dc_bus_OFF */
   );
   if (err_code != 0)
  	  return (err_code);
/*******************************************************************************
 * 
 * 6) Initialize Analog Sensing for AC Motors (ASAC)
 *
 ******************************************************************************/
/*******************************************************************************
 * 6.1) Calculate forgetting factor
 *******************************************************************************
 *  The ASDC function passes the samples through an EWMA filter. The EWMA filter
 *  forgetting factor can be calculated based on the required filter time 
 *  constant according to the following equations:
 *  
 *    filter_time_constant = (effective_filter_length - 1)/sample_frequency
 *  
 *    forgetting_factor = (effective_filter_length - 1)/effective_filter_length
 *  
 *  =>
 *                          filter_time_constant*sample_frequency
 *    forgetting_factor = -----------------------------------------
 *                        filter_time_constant*sample_frequency + 1
 *  
 *    where the sample_frequency is equal to PWM_freq.
 * 
 *
 *  Let's denote: A = filter_time_constant*sample_frequency
 *                B = filter_time_constant*sample_frequency + 1
 *  
 *  Then, using integer arithmetics:
 *    (fract24)forgetting_factor = (A<<23)/B + (((((A<<23)%B)<<8)/B)>>8)
 *  
 ******************************************************************************/
   A = ASAC_filter_time_constant_i_us*PWM_freq_hz;
   B = (ASAC_filter_time_constant_i_us*PWM_freq_hz + 1000000)>>8;
   ASAC_forget_factor_i = ((A/B)<<15) + ((((A%B)<<8)/B)<<7);
   A = ASAC_filter_time_constant_u_us*PWM_freq_hz;
   B = (ASAC_filter_time_constant_u_us*PWM_freq_hz + 1000000)>>8;
   ASAC_forget_factor_u = ((A/B)<<15) + ((((A%B)<<8)/B)<<7);
 /******************************************************************************
 * 6.2) Calculate edgge_offset
 *******************************************************************************
 * trigger the A/D converter queue 16ns before the PWM period start due to ETRIG
 * filter in eQADC module
 *******************************************************************************/
   ASAC_edge_offset = (16*(etpu_tcr1_freq/1000)/1000000);
 /******************************************************************************
 * 6.3) eQADC conversion commands table definition
 *******************************************************************************
 *          trig  ADC0:     ADC1:
 *  CFIFO2: ----> iA/iB/iC  iB/iC/iA - dynamically set according to sector value
 *                iC/iA/iB  
 *                UdcBus
 *                UsinA     UcosA
 *          ----> UsinB     UcosB
 *                UsinB     UcosB    - 4 values needed in order to have iA/B/C 
 *                                     updated next cycle
 *  RFIFO0: iA   UsinA    UsinB   UsinB
 *  RFIFO1: iB   UcosA    UcosB   UcosB
 *  RFIFO2: iC   UdcBus
 ******************************************************************************/
   eQADC_cmds_current_measurements.ADC0_iA_cmd = 0x01000200; /* iA ADC0 RFIFO0 */
   eQADC_cmds_current_measurements.ADC1_iA_cmd = 0x03000200; /* iA ADC1 RFIFO0 */
   eQADC_cmds_current_measurements.ADC0_iB_cmd = 0x01100300; /* iB ADC0 RFIFO1 */
   eQADC_cmds_current_measurements.ADC1_iB_cmd = 0x03100300; /* iB ADC1 RFIFO1 */
   eQADC_cmds_current_measurements.ADC0_iC_cmd = 0x01200400; /* iC ADC0 RFIFO2 */
   eQADC_cmds_current_measurements.ADC1_iC_cmd = 0x03200400; /* iC ADC1 RFIFO2 */
   eQADC_cmds_other_measurements[0] = 0x01200000; /* UdcBus ADC0 RFIFO2 */
   eQADC_cmds_other_measurements[1] = 0x01010600; /* UsinA  ADC0 RFIFO0 */
   eQADC_cmds_other_measurements[2] = 0x83110500; /* UcosA  ADC1 RFIFO1 EOQ */
   eQADC_cmds_other_measurements[3] = 0x01010600; /* UsinB  ADC0 RFIFO0 */
   eQADC_cmds_other_measurements[4] = 0x03110500; /* UcosB  ADC1 RFIFO1 */
   eQADC_cmds_other_measurements[5] = 0x01010600; /* UsinB  ADC0 RFIFO0 */
   eQADC_cmds_other_measurements[6] = 0x83110500; /* UcosB  ADC1 RFIFO1 EOQ */
/*******************************************************************************
 * 6.4) Initialize ASAC function
 ******************************************************************************/
   err_code = fs_etpu_asac_init(
      ASAC_channel, /* channel */
      FS_ETPU_PRIORITY_MIDDLE, /* priority */
      ASAC_polarity, /* polarity */
      FS_ETPU_ASAC_MODE_SYNC, /* mode */
      FS_ETPU_ASAC_SAMPLE_A_B_C_D, /* measure_samples_mask */
      FS_ETPU_ASAC_DTC_ON,	/* DTC_option */
      FS_ETPU_ASAC_PHASE_CURR_USE_SECTOR,	/* phase_current_option */
      FS_ETPU_ASAC_PHASE_CURRENTS_NEG, /* negate_phase_currents */
      FS_ETPU_ASAC_CFIFO_UPDATE_ON,	/* cfifo_update */
      FS_ETPU_ASAC_DMA_INTR_NONE, /* DMA_interrupt_option */
      PWM_period, /* period */
      0, /* start_offset */
      ASAC_edge_offset, /* egde_offset */
      PWM_master_channel, /* PWMMAC_chan */
      (ASAC_measure_time_us * etpu_tcr1_freq)/1000000, /* measure_time */
      PWM_freq_hz/SC_freq_hz, /* periods_per_outerloop */
      SC_channel, /* SC_BC_outerloop_chan */
      RSLV_channel, /* PMSMVC_ACIMVC_innerloop_chan */
      ASAC_result_queue, /* pointer to result queue */
      0, /* queue_offset_a */
      8, /* queue_offset_b */
      16, /* queue_offset_c */
      18, /* queue_offset_d */
      ASAC_forget_factor_i, /* forget_factor_a */
      ASAC_forget_factor_i, /* forget_factor_b */
      ASAC_forget_factor_i, /* forget_factor_c */
      ASAC_forget_factor_u, /* forget_factor_d */
      ASAC_bit_shift, /* bit_shift */
      0x028F5C,	/* dtc_threshold: 0.02 */
      PMSMVC_channel, /* outputA_chan */ 
      FS_ETPU_PMSMVC_IABC_OFFSET, /* outputA_offset */
      PMSMVC_channel, /* outputB_chan */
      FS_ETPU_PMSMVC_IABC_OFFSET + 4, /* outputB_offset */
      PMSMVC_channel, /* outputC_chan */
      FS_ETPU_PMSMVC_IABC_OFFSET + 8, /* outputC_offset */
      PMSMVC_channel, /* outputD1_chan */
      FS_ETPU_PMSMVC_UDCBUSACTUAL_OFFSET, /* outputD1_offset */
      BC_channel, /* outputD2_chan */
      FS_ETPU_BC_UDCBUSMEASURED_OFFSET, /* outputD2_offset */
      10, /* eQADC_cmds_number */
      &eQADC_cmds_current_measurements, /* *eQADC_cmds_current_measurements */
      &eQADC_cmds_other_measurements[0]	/* *eQADC_cmds_other_measurements */
   );
   if (err_code != 0)
  	  return (err_code);

/*******************************************************************************
 * 7) Fill the pmsmrsvc_instance structure
 ******************************************************************************/
   pmsmrsvc_instance->PWM_master_channel     = PWM_master_channel;
   pmsmrsvc_instance->PWM_phaseA_channel     = PWM_phaseA_channel;
   pmsmrsvc_instance->PWM_phaseB_channel     = PWM_phaseB_channel;
   pmsmrsvc_instance->PWM_phaseC_channel     = PWM_phaseC_channel;
   pmsmrsvc_instance->RSLV_channel           = RSLV_channel;
   pmsmrsvc_instance->SC_channel             = SC_channel;
   pmsmrsvc_instance->BC_channel             = BC_channel;
   pmsmrsvc_instance->PMSMVC_channel         = PMSMVC_channel;
   pmsmrsvc_instance->ASAC_channel           = ASAC_channel;
   pmsmrsvc_instance->speed_range_rpm        = speed_range_rpm;
   pmsmrsvc_instance->speed_min_rpm          = speed_min_rpm;
   pmsmrsvc_instance->dc_bus_voltage_range_mv= dc_bus_voltage_range_mv;
   pmsmrsvc_instance->phase_current_range_ma = phase_current_range_ma;
   pmsmrsvc_instance->motor_pole_pairs       = motor_pole_pairs;
   pmsmrsvc_instance->rslv_pole_pairs        = rslv_pole_pairs;
   pmsmrsvc_instance->PMSM_Ke_mv_per_krpm    = PMSM_Ke_mv_per_krpm;
   pmsmrsvc_instance->BC_u_dc_bus_ON_perc    = BC_u_dc_bus_ON_perc;
   pmsmrsvc_instance->BC_u_dc_bus_OFF_perc   = BC_u_dc_bus_OFF_perc;
   pmsmrsvc_instance->ASAC_forget_factor_i   = ASAC_forget_factor_i;
   pmsmrsvc_instance->PWM_freq_hz            = PWM_freq_hz;

   return(0);  
}


/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_calib_start
*PURPOSE      : Turn PWM top channels on and bottom channels off to enable
*               DC-offset calibration of phase currents.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*
*RETURNS NOTES: This function returns 0 if PWM outputs were successfully
*               set. In case PWM channels have any pending HSRs, outputs
*               stayes disabled and this function should be called again later.
*               In this case the pending HSR numbers is returned.
*******************************************************************************/
int32_t fs_etpu_app_pmsmrsvc_calib_start( pmsmrsvc_instance_t * pmsmrsvc_instance)
{
   uint32_t  hsrr;

   /* Reset and turn off D and Q PI controllers */
   fs_etpu_pmsmvc_set_configuration(pmsmrsvc_instance->PMSMVC_channel, FS_ETPU_PMSMVC_PID_OFF);
   fs_etpu_pmsmvc_set_i_d_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_i_q_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_d(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_q(pmsmrsvc_instance->PMSMVC_channel, 0);

   /* Measure 3 phase currents independently on sector value */
   eTPU->CHAN[pmsmrsvc_instance->ASAC_channel].SCR.B.FM0 = 0;

   /* PWM outputs - top off, bottom on */
   hsrr = fs_etpu_pwmmac_disable_3ph(pmsmrsvc_instance->PWM_master_channel,
                                     FS_ETPU_PWMMAC_PIN_HIGH,
                                     FS_ETPU_PWMMAC_PIN_HIGH);
   if (hsrr != 0) return ((int32_t)hsrr);
                               
   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_calib_finish
*PURPOSE      : Use ASAC outputs as phase current DC-offsets, calibrate DC-bus 
*               Break Controller.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*
*RETURNS NOTES: None.
*******************************************************************************/
void fs_etpu_app_pmsmrsvc_calib_finish( pmsmrsvc_instance_t * pmsmrsvc_instance)
{
   /* Use measured currents for DC-offset calibration */
   fs_etpu_asac_measure_dc_offsets(pmsmrsvc_instance->ASAC_channel, FS_ETPU_ASAC_DC_OFFSET_SAMPLE_A_B_C);
   /* Set DC-bus DC-offset to 0 */
   fs_etpu_asac_set_dc_offset_D(pmsmrsvc_instance->ASAC_channel, 0);

   /* Restore option FS_ETPU_ASAC_FM_PHASE_CURRENTS_ON */
   eTPU->CHAN[pmsmrsvc_instance->ASAC_channel].SCR.B.FM0 = FS_ETPU_ASAC_FM_PHASE_CURRENTS_ON;

   /* Set Break Controller threshold values */
   fs_etpu_bc_set_thresholds(pmsmrsvc_instance->BC_channel, 
     (pmsmrsvc_instance->BC_u_dc_bus_ON_perc)*(fs_etpu_asac_get_outputD(pmsmrsvc_instance->ASAC_channel))/100,
     (pmsmrsvc_instance->BC_u_dc_bus_OFF_perc)*(fs_etpu_asac_get_outputD(pmsmrsvc_instance->ASAC_channel))/100); 

}

/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_align_start
*PURPOSE      : Turn PWM top channels on and bottom channels off to enable
*               DC-offset calibration of phase currents.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*   alignment_u_d      - This is the D-voltage used for motor alignment 
*                        as a 24-bit signed fractional value.
*
*RETURNS NOTES: This function returns 0 if PWM phases were successfully
*               enabled. In case phase channels have any pending HSRs, phases
*               are not enabled and this function should be called again later.
*               In this case the pending HSR numbers is returned.
*******************************************************************************/
int32_t fs_etpu_app_pmsmrsvc_align_start( pmsmrsvc_instance_t * pmsmrsvc_instance,
                                                        int24_t   alignment_u_d)
{
   uint32_t hsrr;

   /* Return 0 angle from RSLV during alignment */
   fs_etpu_rslv_set_position(pmsmrsvc_instance->RSLV_channel, 0);

   /* Set required alignment current i_d */
   fs_etpu_pmsmvc_set_i_d_desired(pmsmrsvc_instance->PMSMVC_channel, alignment_u_d);

   /* Enable generation of PWM signals and set their polarity */
   hsrr = fs_etpu_pwmmac_enable_3ph(pmsmrsvc_instance->PWM_master_channel,
                                    FS_ETPU_PWMMAC_ACTIVE_LOW,
                                    FS_ETPU_PWMMAC_ACTIVE_LOW);
   if (hsrr != 0) return ((int32_t)hsrr);
                               
   return(0);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_align_finish
*PURPOSE      : Turn PWM top channels on and bottom channels off to enable
*               DC-offset calibration of phase currents.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*   sc_configuration   - This is the required configuration of SC.
*                        This parameter should be assigned a value of:
*                        FS_ETPU_APP_PMSMRSVC_SPEED_LOOP_OPENED or
*                        FS_ETPU_APP_PMSMRSVC_SPEED_LOOP_CLOSED.
*
*RETURNS NOTES: This function returns 0 if PWM phases were successfully
*               enabled. In case phase channels have any pending HSRs, phases
*               are not enabled and this function should be called again later.
*               In this case the pending HSR numbers is returned.
*******************************************************************************/
void fs_etpu_app_pmsmrsvc_align_finish( pmsmrsvc_instance_t * pmsmrsvc_instance,
                                                      uint8_t   sc_configuration)
{
   /* Reset RSLC position */
   fs_etpu_rslv_set_position(pmsmrsvc_instance->RSLV_channel, 0);

   /* Reset alignment current i_d */
   fs_etpu_pmsmvc_set_i_d_desired(pmsmrsvc_instance->PMSMVC_channel, 0);

   /* Reset required speed, SC-PI integral portion and set SC configuration */
   fs_etpu_sc_set_omega_desired(pmsmrsvc_instance->SC_channel, 0);
   fs_etpu_sc_set_integral_portion(pmsmrsvc_instance->SC_channel, 0);
   fs_etpu_sc_set_configuration(pmsmrsvc_instance->SC_channel, sc_configuration);

   /* Reset required alignment current i_d and turn on D and Q PI controllers */
   fs_etpu_pmsmvc_set_i_d_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_i_q_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_d(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_q(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_configuration(pmsmrsvc_instance->PMSMVC_channel, FS_ETPU_PMSMVC_PID_ON);
}

/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_disable
*PURPOSE      : To disable generation of PWM signals and stop speed and
*               current controllers.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*
*RETURNS NOTES: This function returns 0 if PWM phases were successfully
*               disabled. In case phase channels have any pending HSRs, phases
*               are not disabled and this function should be called again later.
*               In this case the pending HSR numbers is returned.
*******************************************************************************/
int32_t fs_etpu_app_pmsmrsvc_disable( pmsmrsvc_instance_t * pmsmrsvc_instance)
{
   uint32_t hsrr;

   /* Disable generation of PWM signals and set disable pin states */
   hsrr = fs_etpu_pwmmac_disable_3ph(pmsmrsvc_instance->PWM_master_channel,
                                     FS_ETPU_PWMMAC_PIN_HIGH,
                                     FS_ETPU_PWMMAC_PIN_LOW);

   if (hsrr != 0) return ((int32_t)hsrr);

   /* Set SC Ramp and PI off, reset required speed and SC-PI integral portion */
   fs_etpu_sc_set_configuration(pmsmrsvc_instance->SC_channel,
                                FS_ETPU_SC_RAMP_OFF_PID_OFF);
   fs_etpu_sc_set_omega_desired(pmsmrsvc_instance->SC_channel, 0);
   fs_etpu_sc_set_integral_portion(pmsmrsvc_instance->SC_channel, 0);
   
   /* Reset D and Q PI controllers */
   fs_etpu_pmsmvc_set_configuration(pmsmrsvc_instance->PMSMVC_channel,
                                    FS_ETPU_PMSMVC_PID_OFF);
   fs_etpu_pmsmvc_set_i_d_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_i_q_desired(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_d(pmsmrsvc_instance->PMSMVC_channel, 0);
   fs_etpu_pmsmvc_set_integral_portion_q(pmsmrsvc_instance->PMSMVC_channel, 0);

   return(0);
}


/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_set_speed_required
*PURPOSE      : To set the required motor speed.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to bldcmesl1_instance_t structure, 
*                        which is filled by fs_etpu_app_pmsmrsvc_init.
*   speed_required_rpm - This is the required motor speed in rpm.
*                        In case the speed loop is opened (sc_configuration has 
*                        been set to FS_ETPU_APP_PMSMRSVC_SPEED_LOOP_OPENED in 
*                        fs_etpu_pmsmrsvc_align_finish(...)), the required speed 
*                        value is passed directly to the speed controller 
*                        output, which is the required motor torque. 
*                        In this case, the required motor speed in RPM, as 
*                        a fraction of the speed range in RPM, corresponds to 
*                        the required motor torque, as a fraction of the maximum
*                        motor torque.
*******************************************************************************/
void fs_etpu_app_pmsmrsvc_set_speed_required( pmsmrsvc_instance_t * pmsmrsvc_instance,
                                              int32_t   speed_required_rpm)
{
   /* Set required speed */
   if((speed_required_rpm>=0 ? speed_required_rpm : -speed_required_rpm) 
      > pmsmrsvc_instance->speed_min_rpm)
       fs_etpu_sc_set_omega_desired(pmsmrsvc_instance->SC_channel,
          (speed_required_rpm<<15)/pmsmrsvc_instance->speed_range_rpm <<8);
   else
   {
       fs_etpu_sc_set_omega_desired(pmsmrsvc_instance->SC_channel, 0);
   }
}


/*******************************************************************************
*FUNCTION     : fs_etpu_app_pmsmrsvc_get_data
*PURPOSE      : To get application state data.
*INPUTS NOTES : This function has the following parameters:
*   pmsmrsvc_instance - This is a pointer to pmsmrsvc_instance_t structure,
*                        which was filled by fs_etpu_app_pmsmrsvc_init.
*   pmsmrsvc_data     - This is a pointer to pmsmrsvc_data_t structure of
*                        application state data, which is updated.
*******************************************************************************/
void fs_etpu_app_pmsmrsvc_get_data( pmsmrsvc_instance_t * pmsmrsvc_instance,
                                    pmsmrsvc_data_t * pmsmrsvc_data)
{
   pmsmvc_ab_t u_ab;
   pmsmvc_dq_t i_dq;
   int32_t u_a_mv, u_b_mv, u_ampl_sqr, u_ampl=0;
   int32_t j;

   /* speeds are get from the eTPU in signed 24-bit integer format,
      0x7FFFFF corresponds to speed_range_rpm,
      are transformed to rpm */
   pmsmrsvc_data->speed_ramp_rpm =
      (fs_etpu_sc_get_ramp_output(pmsmrsvc_instance->SC_channel)>>8)
      * pmsmrsvc_instance->speed_range_rpm >>15;
   pmsmrsvc_data->speed_actual_rpm =
      (fs_etpu_sc_get_omega_actual(pmsmrsvc_instance->SC_channel)>>8)
      * pmsmrsvc_instance->speed_range_rpm >>15;
   /* torque is proportional to stator current Q-component:
         torque_required = 3/2*motor_pole_pairs*Ke*i_q_desired
         torque_actual   = 3/2*motor_pole_pairs*Ke*i_q
      i_q currents are get from the eTPU in signed 24-bit integer format,
      0x7FFFFF corresponds to phase_current_range_ma */
   fs_etpu_pmsmvc_get_i_dq_desired(pmsmrsvc_instance->PMSMVC_channel, &i_dq);
   pmsmrsvc_data->torque_required_mnm =
      ((i_dq.q>>8) * pmsmrsvc_instance->phase_current_range_ma >>15) * 
      pmsmrsvc_instance->PMSM_Ke_mv_per_krpm * 
      pmsmrsvc_instance->motor_pole_pairs / 1000 * 3 * 60 / (2*1000);
   fs_etpu_pmsmvc_get_i_dq(pmsmrsvc_instance->PMSMVC_channel, &i_dq);
   pmsmrsvc_data->torque_actual_mnm =
      ((i_dq.q>>8) * pmsmrsvc_instance->phase_current_range_ma >>15) *
      pmsmrsvc_instance->PMSM_Ke_mv_per_krpm * 
      pmsmrsvc_instance->motor_pole_pairs / 1000 * 3 * 60 / (2*1000);
   /* applied voltage is calculated from u_ab components, using a square root
      u_ab voltages are get from the eTPU in signed 24-bit integer format,
      0x7FFFFF corresponds to dc_bus_voltage_range_mv */
   fs_etpu_pmsmvc_get_u_ab(pmsmrsvc_instance->PMSMVC_channel, &u_ab);
   u_a_mv = (u_ab.a>>8) * pmsmrsvc_instance->dc_bus_voltage_range_mv >>16;
   u_b_mv = (u_ab.b>>8) * pmsmrsvc_instance->dc_bus_voltage_range_mv >>16;
   u_ampl_sqr = u_a_mv*u_a_mv + u_b_mv*u_b_mv;
   for(j= 1<<15; j!=0; j>>=1)
   {
      u_ampl = u_ampl + j;
      if( u_ampl*u_ampl > u_ampl_sqr)
         u_ampl = u_ampl - j;
   }
   pmsmrsvc_data->applied_voltage_mv = u_ampl;
   /* revolution counter is a signed 24-bit integer, 
      the bits 24-31 are always 0x00 or 0xFF */
   pmsmrsvc_data->revolution_counter =
      fs_etpu_rslv_get_revolutions(pmsmrsvc_instance->RSLV_channel);
   /* the direction can be either FS_ETPU_APP_PMSMRSVC_DIRECTION_INC (0) or
                                  FS_ETPU_APP_PMSMRSVC_DIRECTION_DEC (1) */
   pmsmrsvc_data->direction =
      pmsmrsvc_data->speed_actual_rpm > 0 ? 
      FS_ETPU_APP_PMSMRSVC_DIRECTION_INC : FS_ETPU_APP_PMSMRSVC_DIRECTION_DEC;
   /* the saturation can be FS_ETPU_APP_PMSMRSVC_SATURATION_NO  (0) or
                            FS_ETPU_APP_PMSMRSVC_SATURATION_POS (1) or
                            FS_ETPU_APP_PMSMRSVC_SATURATION_NEG (2) */
   pmsmrsvc_data->sc_saturation =
      fs_etpu_sc_get_saturation_flag(pmsmrsvc_instance->SC_channel);
   pmsmrsvc_data->d_saturation =
      fs_etpu_pmsmvc_get_saturation_flag_d(pmsmrsvc_instance->PMSMVC_channel);
   pmsmrsvc_data->q_saturation =
      fs_etpu_pmsmvc_get_saturation_flag_q(pmsmrsvc_instance->PMSMVC_channel);
}

/*********************************************************************
 *
 * 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
 ********************************************************************/
