/*******************************************************************************
Copyright (c) 2014 released Microchip Technology Inc. All rights reserved.

Microchip licenses to you the right to use, modify, copy and distribute
Software only when embedded on a Microchip microcontroller or digital signal
controller that is integrated into your product or third party product
(pursuant to the sublicense terms in the accompanying license agreement).

You should refer to the license agreement accompanying this Software for
additional information regarding your rights and obligations.

SOFTWARE AND DOCUMENTATION ARE PROVIDED AS IS WITHOUT WARRANTY OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF
MERCHANTABILITY, TITLE, NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE.
IN NO EVENT SHALL MICROCHIP OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER
CONTRACT, NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR
OTHER LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE OR
CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT OF
SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
(INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
*******************************************************************************/

// *****************************************************************************
// *****************************************************************************
// Section: Included Files
// *****************************************************************************
// *****************************************************************************
#include <stdint.h>
#ifdef __XC16__  // See comments at the top of this header file
    #include <xc.h>
#endif // __XC16__

#include "general.h"
#include "motor_control.h"
#include "estim.h"
#include "user_parms.h"
#include <libq.h> 
#ifdef  MOTOR_A
    #include "motorA.h"
    #define DECIMATE_NOMINAL_SPEED_A MA_NOMINAL_SPEED_RPM*MA_NOPOLESPAIRS/10
    #define NOMINAL_ELECTRICAL_SPEED_A MA_NOMINAL_SPEED_RPM*MA_NOPOLESPAIRS
#endif
#ifdef  MOTOR_B
    #include "motorB.h"
    #define DECIMATE_NOMINAL_SPEED_B MB_NOMINAL_SPEED_RPM*MB_NOPOLESPAIRS/10
    #define NOMINAL_ELECTRICAL_SPEED_B MB_NOMINAL_SPEED_RPM*MB_NOPOLESPAIRS
#endif
/******************************************************************************/
/******************************************************************************/
/*                                                                            */
/*                    typedef definitions                                     */
/*                                                                            */
/******************************************************************************/
/******************************************************************************/

MC_SINCOS_T tSinCosParm;
MC_ALPHABETA_T tClarkBEMF;
MC_DQ_T tParkBEMF;
MC_SINCOS_T tSinCosTheta;

// *****************************************************************************
/* Function:
    Estim()

  Summary:
    Motor speed and angle estimator

  Description:
    Estimation of the speed of the motor and field angle based on inverter
    voltages and motor currents.

  Precondition:
    None.

  Parameters:
    iMotorNumber -  Specifiy the motor number

  Returns:
    None.

  Remarks:
    None.
 */
void Estim(uint16_t iMotorNumber)
{
    int32_t tempInt;

    #ifdef  MOTOR_A
    if(iMotorNumber == MOTOR_A)
    {
        uint16_t iIndex=(tEstimParmA.qDiCounter-7)&0x0007;

        /* dIalpha = Ialpha-oldIalpha,  dIbeta  = Ibeta-oldIbeta
        for lower speed the granularity of differnce is higher - the
        difference is made between 2 sampled values @ 8 ADC ISR cycles */
        if (_Q15abs(tEstimParmA.qVelEstim)< NOMINAL_ELECTRICAL_SPEED_A)
        {

            tEstimParmA.qDIalpha = (tClarkCurrentA.alpha -
                                    tEstimParmA.qLastIalphaHS[iIndex]);
            /* The current difference can exceed the maximum value per 8 ADC ISR
            cycle .The following limitation assures a limitation per low speed -
            up to the nominal speed */
            if (tEstimParmA.qDIalpha > tEstimParmA.qDIlimitLS)
            {
                tEstimParmA.qDIalpha = tEstimParmA.qDIlimitLS;
            }
            if (tEstimParmA.qDIalpha < -tEstimParmA.qDIlimitLS)
            {
                tEstimParmA.qDIalpha = -tEstimParmA.qDIlimitLS;
            }

            tEstimParmA.qVIndalpha = (int16_t)(__builtin_mulss(tMotorEstimParmA.qLsDt,
                                               tEstimParmA.qDIalpha)>>10);

            tEstimParmA.qDIbeta	= (tClarkCurrentA.beta -
                                   tEstimParmA.qLastIbetaHS[iIndex]);
            /* The current difference can exceed the maximum value per 8 ADC ISR cycle
            the following limitation assures a limitation per low speed - up to
            the nominal speed */
            if (tEstimParmA.qDIbeta > tEstimParmA.qDIlimitLS)
            {
                tEstimParmA.qDIbeta = tEstimParmA.qDIlimitLS;
            }
            if (tEstimParmA.qDIbeta < -tEstimParmA.qDIlimitLS)
            {
                tEstimParmA.qDIbeta = -tEstimParmA.qDIlimitLS;
            }
            tEstimParmA.qVIndbeta = (int)(__builtin_mulss(tMotorEstimParmA.qLsDt,
                                          tEstimParmA.qDIbeta)>>10);

        }
        else
        {

            tEstimParmA.qDIalpha = (tClarkCurrentA.alpha -
                           tEstimParmA.qLastIalphaHS[(tEstimParmA.qDiCounter)]);

            /* The current difference can exceed the maximum value per 1 ADC ISR cycle
            the following limitation assures a limitation per high speed - up to
            the maximum speed */
            if (tEstimParmA.qDIalpha > tEstimParmA.qDIlimitHS)
            {
                tEstimParmA.qDIalpha = tEstimParmA.qDIlimitHS;
            }
            if (tEstimParmA.qDIalpha < -tEstimParmA.qDIlimitHS)
            {
                tEstimParmA.qDIalpha = -tEstimParmA.qDIlimitHS;
            }
            tEstimParmA.qVIndalpha = (int)(__builtin_mulss(tMotorEstimParmA.qLsDt,
                                           tEstimParmA.qDIalpha)>>7);

            tEstimParmA.qDIbeta	= (tClarkCurrentA.beta -
                            tEstimParmA.qLastIbetaHS[(tEstimParmA.qDiCounter)]);

            /* The current difference can exceed the maximum value per 1 ADC ISR cycle
            the following limitation assures a limitation per high speed - up to
            the maximum speed */
            if (tEstimParmA.qDIbeta > tEstimParmA.qDIlimitHS)
            {
                tEstimParmA.qDIbeta = tEstimParmA.qDIlimitHS;
            }
            if (tEstimParmA.qDIbeta < -tEstimParmA.qDIlimitHS)
            {
                tEstimParmA.qDIbeta = -tEstimParmA.qDIlimitHS;
            }
            tEstimParmA.qVIndbeta = (int16_t)(__builtin_mulss(tMotorEstimParmA.qLsDt,
                                         tEstimParmA.qDIbeta)>>7);
        }
        /* Update  LastIalpha and LastIbeta */
        tEstimParmA.qDiCounter = (tEstimParmA.qDiCounter+1) & 0x0007;
        tEstimParmA.qLastIalphaHS[tEstimParmA.qDiCounter] = tClarkCurrentA.alpha;
        tEstimParmA.qLastIbetaHS[tEstimParmA.qDiCounter] = tClarkCurrentA.beta;

        /* Stator voltage eqations
        Ualpha = Rs * Ialpha + Ls dIalpha/dt + BEMF
        BEMF = Ualpha - Rs Ialpha - Ls dIalpha/dt */

        tClarkBEMF.alpha = tClarkVoltageA.alpha -
                           (int16_t)(__builtin_mulss( tMotorEstimParmA.qRs, tClarkCurrentA.alpha)	>>14)-
                           tEstimParmA.qVIndalpha;
        /* The multiplication between the Rs and Ialpha was shifted by 14 instead
        of 15 because the Rs value normalized exceeded Q15 range, so it was
        divided by 2 immediatelky after the normalization - in userparms.h */

        /* Ubeta = Rs * Ibeta + Ls dIbeta/dt + BEMF
        BEMF = Ubeta - Rs Ibeta - Ls dIbeta/dt */
        tClarkBEMF.beta	= tClarkVoltageA.beta -
                         (int16_t)(__builtin_mulss( tMotorEstimParmA.qRs, tClarkCurrentA.beta )	>>14)-
                          tEstimParmA.qVIndbeta;

        /* The multiplication between the Rs and Ibeta was shifted by 14 instead of 15
        because the Rs value normalized exceeded Q15 range, so it was divided by 2
        immediatelky after the normalization - in userparms.h */

        MC_CalculateSineCosine_Assembly_Ram((tEstimParmA.qRho + tEstimParmA.qRhoOffset),
                                            &tSinCosTheta);

        /*  Park_BEMF.d =  Clark_BEMF.alpha*cos(Angle) + Clark_BEMF.beta*sin(Rho)
        Park_BEMF.q = -Clark_BEMF.alpha*sin(Angle) + Clark_BEMF.beta*cos(Rho)*/
        MC_TransformPark_Assembly(&tClarkBEMF,&tSinCosTheta,&tParkBEMF);

        /* Filter first order for Esd and Esq
        EsdFilter = 1/TFilterd * Intergal{ (Esd-EsdFilter).dt } */

        tempInt = (int16_t)(tParkBEMF.d - tEstimParmA.qEsdf);
        tEstimParmA.qEsdStateVar+= __builtin_mulss(tempInt, tEstimParmA.qKfilterEsdq);
        tEstimParmA.qEsdf = (int16_t)(tEstimParmA.qEsdStateVar>>15);

        tempInt = (int16_t)(tParkBEMF.q - tEstimParmA.qEsqf);
        tEstimParmA.qEsqStateVar+= __builtin_mulss(tempInt, tEstimParmA.qKfilterEsdq);
        tEstimParmA.qEsqf = (int16_t)(tEstimParmA.qEsqStateVar>>15);

        /* OmegaMr= InvKfi * (Esqf -sgn(Esqf) * Esdf)
        For stability the conditio for low speed */
        if (_Q15abs(tEstimParmA.qVelEstim) > DECIMATE_NOMINAL_SPEED_A)
        {
            if(tEstimParmA.qEsqf>0)
            {
                tempInt = (int)(tEstimParmA.qEsqf - tEstimParmA.qEsdf);
                tEstimParmA.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmA.qInvKFi, tempInt)>>15);
            }
            else
            {
                tempInt = (int)(tEstimParmA.qEsqf + tEstimParmA.qEsdf);
                tEstimParmA.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmA.qInvKFi, tempInt)>>15);
            }
        }
        /* if est speed<10% => condition VelRef<>0 */
        else
        {
            if(tEstimParmA.qVelEstim>0)
            {
                tempInt = (int)(tEstimParmA.qEsqf - tEstimParmA.qEsdf);
                tEstimParmA.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmA.qInvKFi,tempInt)>>15);
            }
            else
            {
                tempInt = (int)(tEstimParmA.qEsqf + tEstimParmA.qEsdf);
                tEstimParmA.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmA.qInvKFi,tempInt)>>15);
            }
        }
        /* the result of the calculation above is shifted left by one because
        initally the value of InvKfi was shifted by 2 after normalizing -
        assuring that extended range of the variable is possible in the
        lookup table the initial value of InvKfi is defined in userparms.h */
        tEstimParmA.qOmegaMr = tEstimParmA.qOmegaMr<<1;

        /* the integral of the angle is the estimated angle */
        tEstimParmA.qRhoStateVar+= __builtin_mulss(tEstimParmA.qOmegaMr, tEstimParmA.qDeltaT);
        tEstimParmA.qRho = (int16_t) (tEstimParmA.qRhoStateVar>>15);

        /* The estiamted speed is a filter value of the above calculated OmegaMr.
        The filter implementation is the same as for BEMF d-q components
        filtering */
        tempInt = (int16_t)(tEstimParmA.qOmegaMr-tEstimParmA.qVelEstim);
        tEstimParmA.qVelEstimStateVar+= __builtin_mulss(tempInt, tEstimParmA.qVelEstimFilterK);
        tEstimParmA.qVelEstim =	(int16_t)(tEstimParmA.qVelEstimStateVar>>15);
    }
    #endif
    #ifdef  MOTOR_B
    if(iMotorNumber == MOTOR_B)
    {
        uint16_t iIndex = (tEstimParmB.qDiCounter-7)&0x0007;

        /* dIalpha = Ialpha-oldIalpha,  dIbeta  = Ibeta-oldIbeta
        for lower speed the granularity of differnce is higher - the
        difference is made between 2 sampled values @ 8 ADC ISR cycles */
        if (_Q15abs(tEstimParmB.qVelEstim)<NOMINAL_ELECTRICAL_SPEED_B)
        {

            tEstimParmB.qDIalpha = (tClarkCurrentB.alpha-tEstimParmB.qLastIalphaHS[iIndex]);

            /* The current difference can exceed the maximum value per 8 ADC ISR
            cycle .The following limitation assures a limitation per low speed -
            up to the nominal speed */
            if (tEstimParmB.qDIalpha > tEstimParmB.qDIlimitLS)
            {
                tEstimParmB.qDIalpha = tEstimParmB.qDIlimitLS;
            }
            if (tEstimParmB.qDIalpha < -tEstimParmB.qDIlimitLS)
            {
                tEstimParmB.qDIalpha = -tEstimParmB.qDIlimitLS;
            }
            tEstimParmB.qVIndalpha = (int16_t)(__builtin_mulss(tMotorEstimParmB.qLsDt,tEstimParmB.qDIalpha)>>10);

            tEstimParmB.qDIbeta	= (tClarkCurrentB.beta -
                                   tEstimParmB.qLastIbetaHS[iIndex]);
            /* The current difference can exceed the maximum value per 8 ADC ISR cycle
            the following limitation assures a limitation per low speed - up to
            the nominal speed */
            if (tEstimParmB.qDIbeta > tEstimParmB.qDIlimitLS)
            {
                tEstimParmB.qDIbeta = tEstimParmB.qDIlimitLS;
            }
            if (tEstimParmB.qDIbeta < -tEstimParmB.qDIlimitLS)
            {
                tEstimParmB.qDIbeta = -tEstimParmB.qDIlimitLS;
            }
            tEstimParmB.qVIndbeta = (int16_t)(__builtin_mulss(tMotorEstimParmB.qLsDt, tEstimParmB.qDIbeta)>>10);

        }
        else
        {

            tEstimParmB.qDIalpha = (tClarkCurrentB.alpha -
                                    tEstimParmB.qLastIalphaHS[(tEstimParmB.qDiCounter)]);
            /* The current difference can exceed the maximum value per 1 ADC ISR cycle
            the following limitation assures a limitation per high speed - up to
            the maximum speed */
            if (tEstimParmB.qDIalpha > tEstimParmB.qDIlimitHS)
            {
                tEstimParmB.qDIalpha = tEstimParmB.qDIlimitHS;
            }
            if (tEstimParmB.qDIalpha < -tEstimParmB.qDIlimitHS)
            {
                tEstimParmB.qDIalpha = -tEstimParmB.qDIlimitHS;
            }
            tEstimParmB.qVIndalpha = (int16_t)(__builtin_mulss(tMotorEstimParmB.qLsDt, tEstimParmB.qDIalpha)>>7);

            tEstimParmB.qDIbeta	= (tClarkCurrentB.beta-tEstimParmB.qLastIbetaHS[(tEstimParmB.qDiCounter)]);
            /* The current difference can exceed the maximum value per 1 ADC ISR cycle
            the following limitation assures a limitation per high speed - up to
            the maximum speed */
            if (tEstimParmB.qDIbeta>tEstimParmB.qDIlimitHS)
            {
                tEstimParmB.qDIbeta=tEstimParmB.qDIlimitHS;
            }
            if (tEstimParmB.qDIbeta<-tEstimParmB.qDIlimitHS)
            {
                tEstimParmB.qDIbeta=-tEstimParmB.qDIlimitHS;
            }
            tEstimParmB.qVIndbeta= (int16_t)(__builtin_mulss(tMotorEstimParmB.qLsDt, tEstimParmB.qDIbeta)>>7);

        }

        /* Update  LastIalpha and LastIbeta */
        tEstimParmB.qDiCounter = (tEstimParmB.qDiCounter+1) & 0x0007;
        tEstimParmB.qLastIalphaHS[tEstimParmB.qDiCounter] = tClarkCurrentB.alpha;
        tEstimParmB.qLastIbetaHS[tEstimParmB.qDiCounter] = tClarkCurrentB.beta;

        /* Stator voltage eqations
        Ualpha = Rs * Ialpha + Ls dIalpha/dt + BEMF
        BEMF = Ualpha - Rs Ialpha - Ls dIalpha/dt */

        tClarkBEMF.alpha =  tClarkVoltageB.alpha -
                            (int16_t)(__builtin_mulss( tMotorEstimParmB.qRs, tClarkCurrentB.alpha)	>>14)-
                            tEstimParmB.qVIndalpha;
        /* The multiplication between the Rs and Ialpha was shifted by 14 instead
        of 15 because the Rs value normalized exceeded Q15 range, so it was
        divided by 2 immediatelky after the normalization - in userparms.h */

        /* Ubeta = Rs * Ibeta + Ls dIbeta/dt + BEMF
        BEMF = Ubeta - Rs Ibeta - Ls dIbeta/dt */

        tClarkBEMF.beta	= tClarkVoltageB.beta -
                          (int16_t)(__builtin_mulss( tMotorEstimParmB.qRs, tClarkCurrentB.beta )	>>14)-
                          tEstimParmB.qVIndbeta;

        /* The multiplication between the Rs and Ibeta was shifted by 14 instead of 15
        because the Rs value normalized exceeded Q15 range, so it was divided by 2
        immediatelky after the normalization - in userparms.h */

        MC_CalculateSineCosine_Assembly_Ram((tEstimParmB.qRho + tEstimParmB.qRhoOffset),
                                            &tSinCosTheta);
        /*  Park_BEMF.d =  Clark_BEMF.alpha*cos(Angle) + Clark_BEMF.beta*sin(Rho)
        Park_BEMF.q = -Clark_BEMF.alpha*sin(Angle) + Clark_BEMF.beta*cos(Rho)*/
        MC_TransformPark_Assembly(&tClarkBEMF,&tSinCosTheta,&tParkBEMF);

        /* Filter first order for Esd and Esq
        EsdFilter = 1/TFilterd * Intergal{ (Esd-EsdFilter).dt } */

        tempInt = (int16_t)(tParkBEMF.d - tEstimParmB.qEsdf);
        tEstimParmB.qEsdStateVar+= __builtin_mulss(tempInt, tEstimParmB.qKfilterEsdq);
        tEstimParmB.qEsdf = (int16_t)(tEstimParmB.qEsdStateVar>>15);

        tempInt = (int16_t)(tParkBEMF.q - tEstimParmB.qEsqf);
        tEstimParmB.qEsqStateVar+= __builtin_mulss(tempInt, tEstimParmB.qKfilterEsdq);
        tEstimParmB.qEsqf = (int16_t)(tEstimParmB.qEsqStateVar>>15);

        /* OmegaMr= InvKfi * (Esqf -sgn(Esqf) * Esdf)
        For stability the conditio for low speed */
        if (_Q15abs(tEstimParmB.qVelEstim) > DECIMATE_NOMINAL_SPEED_B)
        {
            if(tEstimParmB.qEsqf>0)
            {
                tempInt = (int16_t)(tEstimParmB.qEsqf- tEstimParmB.qEsdf);
                tEstimParmB.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmB.qInvKFi, tempInt)>>15);
            }
            else
            {
                tempInt = (int16_t)(tEstimParmB.qEsqf + tEstimParmB.qEsdf);
                tEstimParmB.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmB.qInvKFi, tempInt)>>15);
            }
        }
        /* if est speed<10% => condition VelRef<>0 */
        else
        {
            if(tEstimParmB.qVelEstim>0)
            {
                tempInt = (int16_t)(tEstimParmB.qEsqf - tEstimParmB.qEsdf);
                tEstimParmB.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmB.qInvKFi,tempInt)>>15);
            }
            else
            {
                tempInt = (int16_t)(tEstimParmB.qEsqf+ tEstimParmB.qEsdf);
                tEstimParmB.qOmegaMr = (int16_t)(__builtin_mulss(tMotorEstimParmB.qInvKFi,tempInt)>>15);
            }
        }
        /* the result of the calculation above is shifted left by one because
        initally the value of InvKfi was shifted by 2 after normalizing -
        assuring that extended range of the variable is possible in the
        lookup table the initial value of InvKfi is defined in userparms.h */
        tEstimParmB.qOmegaMr = tEstimParmB.qOmegaMr<<1;

        /* the integral of the angle is the estimated angle */
        tEstimParmB.qRhoStateVar+= __builtin_mulss(tEstimParmB.qOmegaMr, tEstimParmB.qDeltaT);
        tEstimParmB.qRho = (int16_t) (tEstimParmB.qRhoStateVar>>15);

        /* The estiamted speed is a filter value of the above calculated OmegaMr.
        The filter implementation is the same as for BEMF d-q components
        filtering */
        tempInt = (int16_t)(tEstimParmB.qOmegaMr-tEstimParmB.qVelEstim);
        tEstimParmB.qVelEstimStateVar+=__builtin_mulss(tempInt, tEstimParmB.qVelEstimFilterK);
        tEstimParmB.qVelEstim=	(int16_t)(tEstimParmB.qVelEstimStateVar>>15);
    }
    #endif
}
// *****************************************************************************
/* Function:
    InitEstimParm ()

  Summary:
    Initialises Motor speed and angle estimator parameters

  Description:
    Initialisation of the parameters of the estimator.

  Precondition:
    None.

  Parameters:
    iMotorNumber -  Specifiy the motor number

  Returns:
    None.

  Remarks:
    None.
 */
void InitEstimParm(uint16_t Motor_Number)
{
    #ifdef MOTOR_A
    if (Motor_Number == MOTOR_A)
    {
        tMotorEstimParmA.qLsDtBase = MA_NORM_LSDTBASE;
        tMotorEstimParmA.qLsDt = tMotorEstimParmA.qLsDtBase;
        tMotorEstimParmA.qRs = MA_NORM_RS;

        tMotorEstimParmA.qInvKFiBase = MA_NORM_INVKFIBASE;
        tMotorEstimParmA.qInvKFi = tMotorEstimParmA.qInvKFiBase;

        tEstimParmA.qRhoStateVar = 0;
        tEstimParmA.qOmegaMr = 0;
        tEstimParmA.qDiCounter = 0;
        tEstimParmA.qEsdStateVar = 0;
        tEstimParmA.qEsqStateVar = 0;

        tEstimParmA.qDIlimitHS = MA_D_ILIMIT_HS;
        tEstimParmA.qDIlimitLS = MA_D_ILIMIT_LS;

        tEstimParmA.qKfilterEsdq = MA_KFILTER_ESDQ;
        tEstimParmA.qVelEstimFilterK = MA_KFILTER_VELESTIM;

        tEstimParmA.qDeltaT = MA_NORM_DELTAT;
        tEstimParmA.qRhoOffset = INITOFFSET_TRANS_OPEN_CLSD_A;
    }
    #endif
    #ifdef MOTOR_B
    else if (Motor_Number == MOTOR_B)
    {

        tMotorEstimParmB.qLsDtBase = MB_NORM_LSDTBASE;
        tMotorEstimParmB.qLsDt = tMotorEstimParmB.qLsDtBase;
        tMotorEstimParmB.qRs = MB_NORM_RS;

        tMotorEstimParmB.qInvKFiBase = MB_NORM_INVKFIBASE;
        tMotorEstimParmB.qInvKFi = tMotorEstimParmB.qInvKFiBase;

        tEstimParmB.qRhoStateVar = 0;
        tEstimParmB.qOmegaMr = 0;
        tEstimParmB.qDiCounter = 0;
        tEstimParmB.qEsdStateVar = 0;
        tEstimParmB.qEsqStateVar = 0;

        tEstimParmB.qDIlimitHS = MB_D_ILIMIT_HS;
        tEstimParmB.qDIlimitLS = MB_D_ILIMIT_LS;

        tEstimParmB.qKfilterEsdq = MB_KFILTER_ESDQ;
        tEstimParmB.qVelEstimFilterK = MB_KFILTER_VELESTIM;

        tEstimParmB.qDeltaT = MB_NORM_DELTAT;
        tEstimParmB.qRhoOffset = INITOFFSET_TRANS_OPEN_CLSD_B;
    }
    #endif
}
