/*******************************************************************************
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    "fdweak.h"
#include    "estim.h"
#include    "user_parms.h"
#ifdef MOTOR_A
    #include    "motorA.h"
    FDWEAK_PARM_T  tFdWeakParmA;
    #define FWONSPEED_A MA_NOMINAL_SPEED_RPM*MA_NOPOLESPAIRS
#endif
#ifdef MOTOR_B
    #include    "motorB.h"
    FDWEAK_PARM_T  tFdWeakParmB;
    #define FWONSPEED_B MB_NOMINAL_SPEED_RPM*MB_NOPOLESPAIRS
#endif

// *****************************************************************************
/* Function:
    InitFWParams()

  Summary:
    Initialises field weakening parameters

  Description:
    This routine initilases field weakening structure variables

  Precondition:
    None.

  Parameters:
    iMotorNumber -  Specifiy the motor number

  Returns:
    None.

  Remarks:
    None.
 */
void InitFWParams(uint16_t iMotorNumber)
{
    #ifdef MOTOR_A
    if(iMotorNumber == MOTOR_A)
    {
        /* Field Weakening constant for constant torque range */
        /* Flux reference value */
        tFdWeakParmA.qIdRef = MA_IDREF_BASESPEED;
        /* Startspeed for Fieldweakening */
        tFdWeakParmA.qFwOnSpeed = FWONSPEED_A -(1<<MA_SPEED_INDEX_CONST);

        /* Initialize magnetizing curve values */
        tFdWeakParmA.qFwCurve[0] = MA_IDREF_SPEED0;
        tFdWeakParmA.qFwCurve[1] = MA_IDREF_SPEED1;
        tFdWeakParmA.qFwCurve[2] = MA_IDREF_SPEED2;
        tFdWeakParmA.qFwCurve[3] = MA_IDREF_SPEED3;
        tFdWeakParmA.qFwCurve[4] = MA_IDREF_SPEED4;
        tFdWeakParmA.qFwCurve[5] = MA_IDREF_SPEED5;
        tFdWeakParmA.qFwCurve[6] = MA_IDREF_SPEED6;
        tFdWeakParmA.qFwCurve[7] = MA_IDREF_SPEED7;
        tFdWeakParmA.qFwCurve[8] = MA_IDREF_SPEED8;
        tFdWeakParmA.qFwCurve[9] = MA_IDREF_SPEED9;
        tFdWeakParmA.qFwCurve[10] = MA_IDREF_SPEED10;
        tFdWeakParmA.qFwCurve[11] = MA_IDREF_SPEED11;
        tFdWeakParmA.qFwCurve[12] = MA_IDREF_SPEED12;
        tFdWeakParmA.qFwCurve[13] = MA_IDREF_SPEED13;
        tFdWeakParmA.qFwCurve[14] = MA_IDREF_SPEED14;
        tFdWeakParmA.qFwCurve[15] = MA_IDREF_SPEED15;
        tFdWeakParmA.qFwCurve[16] = MA_IDREF_SPEED16;
        tFdWeakParmA.qFwCurve[17] = MA_IDREF_SPEED17;

        /* Init inverse Kfi curve values */
        tFdWeakParmA.qInvKFiCurve[0] = MA_INVKFI_SPEED0;
        tFdWeakParmA.qInvKFiCurve[1] = MA_INVKFI_SPEED1;
        tFdWeakParmA.qInvKFiCurve[2] = MA_INVKFI_SPEED2;
        tFdWeakParmA.qInvKFiCurve[3] = MA_INVKFI_SPEED3;
        tFdWeakParmA.qInvKFiCurve[4] = MA_INVKFI_SPEED4;
        tFdWeakParmA.qInvKFiCurve[5] = MA_INVKFI_SPEED5;
        tFdWeakParmA.qInvKFiCurve[6] = MA_INVKFI_SPEED6;
        tFdWeakParmA.qInvKFiCurve[7] = MA_INVKFI_SPEED7;
        tFdWeakParmA.qInvKFiCurve[8] = MA_INVKFI_SPEED8;
        tFdWeakParmA.qInvKFiCurve[9] = MA_INVKFI_SPEED9;
        tFdWeakParmA.qInvKFiCurve[10] = MA_INVKFI_SPEED10;
        tFdWeakParmA.qInvKFiCurve[11] = MA_INVKFI_SPEED11;
        tFdWeakParmA.qInvKFiCurve[12] = MA_INVKFI_SPEED12;
        tFdWeakParmA.qInvKFiCurve[13] = MA_INVKFI_SPEED13;
        tFdWeakParmA.qInvKFiCurve[14] = MA_INVKFI_SPEED14;
        tFdWeakParmA.qInvKFiCurve[15] = MA_INVKFI_SPEED15;
        tFdWeakParmA.qInvKFiCurve[16] = MA_INVKFI_SPEED16;
        tFdWeakParmA.qInvKFiCurve[17] = MA_INVKFI_SPEED17;

        /* Init Ls variation curve */
        tFdWeakParmA.qLsCurve[0] = MA_LS_OVER2LS0_SPEED0;
        tFdWeakParmA.qLsCurve[1] = MA_LS_OVER2LS0_SPEED1;
        tFdWeakParmA.qLsCurve[2] = MA_LS_OVER2LS0_SPEED2;
        tFdWeakParmA.qLsCurve[3] = MA_LS_OVER2LS0_SPEED3;
        tFdWeakParmA.qLsCurve[4] = MA_LS_OVER2LS0_SPEED4;
        tFdWeakParmA.qLsCurve[5] = MA_LS_OVER2LS0_SPEED5;
        tFdWeakParmA.qLsCurve[6] = MA_LS_OVER2LS0_SPEED6;
        tFdWeakParmA.qLsCurve[7] = MA_LS_OVER2LS0_SPEED7;
        tFdWeakParmA.qLsCurve[8] = MA_LS_OVER2LS0_SPEED8;
        tFdWeakParmA.qLsCurve[9] = MA_LS_OVER2LS0_SPEED9;
        tFdWeakParmA.qLsCurve[10] = MA_LS_OVER2LS0_SPEED10;
        tFdWeakParmA.qLsCurve[11] = MA_LS_OVER2LS0_SPEED11;
        tFdWeakParmA.qLsCurve[12] = MA_LS_OVER2LS0_SPEED12;
        tFdWeakParmA.qLsCurve[13] = MA_LS_OVER2LS0_SPEED13;
        tFdWeakParmA.qLsCurve[14] = MA_LS_OVER2LS0_SPEED14;
        tFdWeakParmA.qLsCurve[15] = MA_LS_OVER2LS0_SPEED15;
        tFdWeakParmA.qLsCurve[16] = MA_LS_OVER2LS0_SPEED16;
        tFdWeakParmA.qLsCurve[17] = MA_LS_OVER2LS0_SPEED17;
    }
    #endif
    #ifdef MOTOR_B
    if(iMotorNumber == MOTOR_B)
    {
        /* Field Weakening constant for constant torque range */
        /* Flux reference value */
        tFdWeakParmB.qIdRef = MB_IDREF_BASESPEED;
        /* Startspeed for Fieldweakening */
        tFdWeakParmB.qFwOnSpeed=FWONSPEED_B-(1<<MB_SPEED_INDEX_CONST);

        /* Initialize magnetizing curve values */
        tFdWeakParmB.qFwCurve[0] = MB_IDREF_SPEED0;
        tFdWeakParmB.qFwCurve[1] = MB_IDREF_SPEED1;
        tFdWeakParmB.qFwCurve[2] = MB_IDREF_SPEED2;
        tFdWeakParmB.qFwCurve[3] = MB_IDREF_SPEED3;
        tFdWeakParmB.qFwCurve[4] = MB_IDREF_SPEED4;
        tFdWeakParmB.qFwCurve[5] = MB_IDREF_SPEED5;
        tFdWeakParmB.qFwCurve[6] = MB_IDREF_SPEED6;
        tFdWeakParmB.qFwCurve[7] = MB_IDREF_SPEED7;
        tFdWeakParmB.qFwCurve[8] = MB_IDREF_SPEED8;
        tFdWeakParmB.qFwCurve[9] = MB_IDREF_SPEED9;
        tFdWeakParmB.qFwCurve[10] = MB_IDREF_SPEED10;
        tFdWeakParmB.qFwCurve[11] = MB_IDREF_SPEED11;
        tFdWeakParmB.qFwCurve[12] = MB_IDREF_SPEED12;
        tFdWeakParmB.qFwCurve[13] = MB_IDREF_SPEED13;
        tFdWeakParmB.qFwCurve[14] = MB_IDREF_SPEED14;
        tFdWeakParmB.qFwCurve[15] = MB_IDREF_SPEED15;
        tFdWeakParmB.qFwCurve[16] = MB_IDREF_SPEED16;
        tFdWeakParmB.qFwCurve[17] = MB_IDREF_SPEED17;

        /* Init inverse Kfi curve values */
        tFdWeakParmB.qInvKFiCurve[0] = MB_INVKFI_SPEED0;
        tFdWeakParmB.qInvKFiCurve[1] = MB_INVKFI_SPEED1;
        tFdWeakParmB.qInvKFiCurve[2] = MB_INVKFI_SPEED2;
        tFdWeakParmB.qInvKFiCurve[3] = MB_INVKFI_SPEED3;
        tFdWeakParmB.qInvKFiCurve[4] = MB_INVKFI_SPEED4;
        tFdWeakParmB.qInvKFiCurve[5] = MB_INVKFI_SPEED5;
        tFdWeakParmB.qInvKFiCurve[6] = MB_INVKFI_SPEED6;
        tFdWeakParmB.qInvKFiCurve[7] = MB_INVKFI_SPEED7;
        tFdWeakParmB.qInvKFiCurve[8] = MB_INVKFI_SPEED8;
        tFdWeakParmB.qInvKFiCurve[9] = MB_INVKFI_SPEED9;
        tFdWeakParmB.qInvKFiCurve[10] = MB_INVKFI_SPEED10;
        tFdWeakParmB.qInvKFiCurve[11] = MB_INVKFI_SPEED11;
        tFdWeakParmB.qInvKFiCurve[12] = MB_INVKFI_SPEED12;
        tFdWeakParmB.qInvKFiCurve[13] = MB_INVKFI_SPEED13;
        tFdWeakParmB.qInvKFiCurve[14] = MB_INVKFI_SPEED14;
        tFdWeakParmB.qInvKFiCurve[15] = MB_INVKFI_SPEED15;
        tFdWeakParmB.qInvKFiCurve[16] = MB_INVKFI_SPEED16;
        tFdWeakParmB.qInvKFiCurve[17] = MB_INVKFI_SPEED17;

        /* Init Ls variation curve */
        tFdWeakParmB.qLsCurve[0] = MB_LS_OVER2LS0_SPEED0;
        tFdWeakParmB.qLsCurve[1] = MB_LS_OVER2LS0_SPEED1;
        tFdWeakParmB.qLsCurve[2] = MB_LS_OVER2LS0_SPEED2;
        tFdWeakParmB.qLsCurve[3] = MB_LS_OVER2LS0_SPEED3;
        tFdWeakParmB.qLsCurve[4] = MB_LS_OVER2LS0_SPEED4;
        tFdWeakParmB.qLsCurve[5] = MB_LS_OVER2LS0_SPEED5;
        tFdWeakParmB.qLsCurve[6] = MB_LS_OVER2LS0_SPEED6;
        tFdWeakParmB.qLsCurve[7] = MB_LS_OVER2LS0_SPEED7;
        tFdWeakParmB.qLsCurve[8] = MB_LS_OVER2LS0_SPEED8;
        tFdWeakParmB.qLsCurve[9] = MB_LS_OVER2LS0_SPEED9;
        tFdWeakParmB.qLsCurve[10] = MB_LS_OVER2LS0_SPEED10;
        tFdWeakParmB.qLsCurve[11] = MB_LS_OVER2LS0_SPEED11;
        tFdWeakParmB.qLsCurve[12] = MB_LS_OVER2LS0_SPEED12;
        tFdWeakParmB.qLsCurve[13] = MB_LS_OVER2LS0_SPEED13;
        tFdWeakParmB.qLsCurve[14] = MB_LS_OVER2LS0_SPEED14;
        tFdWeakParmB.qLsCurve[15] = MB_LS_OVER2LS0_SPEED15;
        tFdWeakParmB.qLsCurve[16] = MB_LS_OVER2LS0_SPEED16;
        tFdWeakParmB.qLsCurve[17] = MB_LS_OVER2LS0_SPEED17;
    }
    #endif
}
// *****************************************************************************
/* Function:
    FieldWeakening()

  Summary:
    Routine impliments field weakening

  Description:
    Function caluclates the Id reference based on the motor speed

  Precondition:
    None.

  Parameters:
    Motor Speed
    iMotorNumber -  Specifiy the motor number

  Returns:
    Id reference.

  Remarks:
    None.
 */
int16_t	FieldWeakening(uint16_t iMotorNumber,int16_t qMotorSpeed)
{
    int16_t tempInt1, tempInt2;
    int16_t qInvKFi;
    int16_t qLsDt;

    #ifdef MOTOR_A
    if(iMotorNumber == MOTOR_A)
    {
        /* LsDt value - for base speed */
        qLsDt=tMotorEstimParmA.qLsDtBase;

        /* if the speed is less than one for activating the FW */
        if (qMotorSpeed <= tFdWeakParmA.qFwOnSpeed)
        {
            /* set Idref as first value in magnetizing curve */
            tFdWeakParmA.qIdRef = tFdWeakParmA.qFwCurve[0];

            /* adapt fileter parameter */
            tEstimParmA.qKfilterEsdq = MA_KFILTER_ESDQ;

            /* inverse Kfi constant for base speed */
            qInvKFi = tMotorEstimParmA.qInvKFiBase;
        }
        else
        {
            /* Get the index parameter */
            /* Index in FW-Table */
            tFdWeakParmA.qIndex = (qMotorSpeed - tFdWeakParmA.qFwOnSpeed)>>MA_SPEED_INDEX_CONST;

            tempInt1 = tFdWeakParmA.qFwCurve[tFdWeakParmA.qIndex]-
                       tFdWeakParmA.qFwCurve[tFdWeakParmA.qIndex+1];
            tempInt2 = (tFdWeakParmA.qIndex<<MA_SPEED_INDEX_CONST)+
                        tFdWeakParmA.qFwOnSpeed;
            tempInt2 = qMotorSpeed - tempInt2;

            /* Interpolation betwen two results from the Table */
            tFdWeakParmA.qIdRef = tFdWeakParmA.qFwCurve[tFdWeakParmA.qIndex]-
                                  (int16_t)(__builtin_mulss(tempInt1, tempInt2)>>MA_SPEED_INDEX_CONST);

            /* Adapt filer parameter */
            tEstimParmA.qKfilterEsdq = MA_KFILTER_ESDQ_FW;

            /* Interpolation betwen two results from the Table */
            tempInt1 = tFdWeakParmA.qInvKFiCurve[tFdWeakParmA.qIndex]-
                       tFdWeakParmA.qInvKFiCurve[tFdWeakParmA.qIndex+1];
            qInvKFi = tFdWeakParmA.qInvKFiCurve[tFdWeakParmA.qIndex] -
                      (int16_t)(__builtin_mulss(tempInt1, tempInt2)>>MA_SPEED_INDEX_CONST);


            /* Interpolation betwen two results from the Table */
            tempInt1 = tFdWeakParmA.qLsCurve[tFdWeakParmA.qIndex]-
                       tFdWeakParmA.qLsCurve[tFdWeakParmA.qIndex+1];
            tempInt1 = tFdWeakParmA.qLsCurve[tFdWeakParmA.qIndex] -
                       (int16_t)(__builtin_mulss(tempInt1,tempInt2)>>MA_SPEED_INDEX_CONST);

            /* Lsdt = Lsdt0*Ls/Ls0 */
            qLsDt = (int16_t) ( __builtin_mulss(qLsDt,tempInt1)>>14);
        }

        tMotorEstimParmA.qInvKFi = qInvKFi;
        tMotorEstimParmA.qLsDt = qLsDt;
        return tFdWeakParmA.qIdRef;
    }
    #endif
    #ifdef MOTOR_B
    if(iMotorNumber == MOTOR_B)
    {
        /* LsDt value - for base speed */
        qLsDt = tMotorEstimParmB.qLsDtBase;

        /* if the speed is less than one for activating the FW */
        if (qMotorSpeed <= tFdWeakParmB.qFwOnSpeed)
        {
            /* Set Idref as first value in magnetizing curve */
            tFdWeakParmB.qIdRef = tFdWeakParmB.qFwCurve[0];

            /* Adapt fileter parameter */
            tEstimParmB.qKfilterEsdq = MB_KFILTER_ESDQ;

            /* Inverse Kfi constant for base speed */
            qInvKFi = tMotorEstimParmB.qInvKFiBase;
        }
        else
        {
            /* Get the index parameter */
            /* Index in FW-Table */
            tFdWeakParmB.qIndex = (qMotorSpeed-tFdWeakParmB.qFwOnSpeed)>>MB_SPEED_INDEX_CONST;

            tempInt1 = tFdWeakParmB.qFwCurve[tFdWeakParmB.qIndex]-
                       tFdWeakParmB.qFwCurve[tFdWeakParmB.qIndex+1];
            tempInt2 = (tFdWeakParmB.qIndex<<MB_SPEED_INDEX_CONST)+
                        tFdWeakParmB.qFwOnSpeed;
            tempInt2 = qMotorSpeed - tempInt2;

            /* Interpolation betwen two results from the Table */
            tFdWeakParmB.qIdRef = tFdWeakParmB.qFwCurve[tFdWeakParmB.qIndex]-
                                 (int16_t)(__builtin_mulss(tempInt1, tempInt2)>>MB_SPEED_INDEX_CONST);

            /* Adapt filer parameter */
            tEstimParmB.qKfilterEsdq = MB_KFILTER_ESDQ_FW;

            /* Interpolation betwen two results from the Table */
            tempInt1  = tFdWeakParmB.qInvKFiCurve[tFdWeakParmB.qIndex]-
                        tFdWeakParmB.qInvKFiCurve[tFdWeakParmB.qIndex+1];
            qInvKFi = tFdWeakParmB.qInvKFiCurve[tFdWeakParmB.qIndex] -
                      (int16_t)(__builtin_mulss(tempInt1, tempInt2)>>MB_SPEED_INDEX_CONST);

            /* Interpolation betwen two results from the Table */
            tempInt1 = tFdWeakParmB.qLsCurve[tFdWeakParmB.qIndex]-
                       tFdWeakParmB.qLsCurve[tFdWeakParmB.qIndex+1];
            tempInt1 = tFdWeakParmB.qLsCurve[tFdWeakParmB.qIndex] -
                       (int16_t)(__builtin_mulss(tempInt1,tempInt2)>>MB_SPEED_INDEX_CONST);

            /* Lsdt = Lsdt0*Ls/Ls0 */
            qLsDt = (int16_t) ( __builtin_mulss(qLsDt,tempInt1)>>14);
        }
        tMotorEstimParmB.qInvKFi = qInvKFi;
        tMotorEstimParmB.qLsDt = qLsDt;
        return tFdWeakParmB.qIdRef;
    }
    #endif
}


/* EOF */