/*******************************************************************************
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 "field_weak.h"
#include "general.h"
#include "smcpos.h"
#include "periph.h"

T_FD_WEAK_PARM fdWeakParm;

int16_t FieldWeakening(int16_t qMotorSpeed)
{
    /* if the speed is less than one for activating the FW */
    if (qMotorSpeed <= fdWeakParm.qFwOnSpeed)
    {
        /* set Idref as first value in magnetizing curve */
        fdWeakParm.qIdRef = fdWeakParm.qFwCurve[0];
    }
    else
    {
        /* Index in FW-Table. The result is left shifted 11 times because
        /* we have a field weakening table of 16 (4 bits) values, and the result
        /* of the division is 15 bits (16 bits, with no sign). So
        /* Result (15 bits) >> 11 -> Index (4 bits). */
        fdWeakParm.qFWPercentage = FracDiv((qMotorSpeed - fdWeakParm.qFwOnSpeed),   \
							   Q15(OMEGAFIELDWK - OMEGANOMINAL + 1));
        fdWeakParm.qIndex = fdWeakParm.qFWPercentage >> 11;

        /* Interpolation betwen two results from the Table. First mask 11 bits,
        /* then left shift 4 times to get 15 bits again.*/
        fdWeakParm.qInterpolPortion = (fdWeakParm.qFWPercentage & 0x07FF) << 4;

        fdWeakParm.qIdRef = fdWeakParm.qFwCurve[fdWeakParm.qIndex] \
							- FracMpy(fdWeakParm.qFwCurve[fdWeakParm.qIndex] \
									- fdWeakParm.qFwCurve[fdWeakParm.qIndex + 1] \
									 , fdWeakParm.qInterpolPortion);
    }
    return fdWeakParm.qIdRef;
}

void FWInit(void)
{
    /* initialize magnetizing curve values */
    fdWeakParm.qFwOnSpeed = Q15(OMEGANOMINAL);
    fdWeakParm.qFwCurve[0] = DQKF_W0;
    fdWeakParm.qFwCurve[1] = DQKF_W1;
    fdWeakParm.qFwCurve[2] = DQKF_W2;
    fdWeakParm.qFwCurve[3] = DQKF_W3;
    fdWeakParm.qFwCurve[4] = DQKF_W4;
    fdWeakParm.qFwCurve[5] = DQKF_W5;
    fdWeakParm.qFwCurve[6] = DQKF_W6;
    fdWeakParm.qFwCurve[7] = DQKF_W7;
    fdWeakParm.qFwCurve[8] = DQKF_W8;
    fdWeakParm.qFwCurve[9] = DQKF_W9;
    fdWeakParm.qFwCurve[10] = DQKF_W10;
    fdWeakParm.qFwCurve[11] = DQKF_W11;
    fdWeakParm.qFwCurve[12] = DQKF_W12;
    fdWeakParm.qFwCurve[13] = DQKF_W13;
    fdWeakParm.qFwCurve[14] = DQKF_W14;
    fdWeakParm.qFwCurve[15] = DQKF_W15;
    return;
}
