/*
 * Copyright (c) 2022, Shenzhen CVA Innovation CO.,LTD
 * All rights reserved.
 *
 * Shenzhen CVA Innovation CO.,LTD (CVA chip) is supplying this file for use
 * exclusively with CVA's microcontroller products. This file can be freely
 * distributed within development tools that are supporting such microcontroller
 * products.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * CVA SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
 * OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 */

/*******************************************************************************
 * the includes
 ******************************************************************************/

#include "esttrq_tf.h"

/*******************************************************************************
 * the defines
 ******************************************************************************/

/*******************************************************************************
 * the globals
 ******************************************************************************/

/*******************************************************************************
 * the functions
 ******************************************************************************/

void EstTrqTf_Init(EstTrqTfType *obj, const EstTrqTf_ParamsType *pParams)
{
    obj->torqueFactor_I = _IQ(1.5 * pParams->polePairs * pParams->IBase_A / pParams->torqueBase_Nm);
    obj->torqueFactor_F = _IQ((pParams->Ld_mH - pParams->Lq_mH) * 0.001 * pParams->IBase_A);
    obj->flux_Wb        = pParams->flux_Wb;
    obj->IBaseFactor    = 0.001 * pParams->IBase_A;
}

void EstTrqTf_Calc(EstTrqTfType *obj)
{
    obj->torque_pu = _IQmpy(_IQmpy(obj->filteredIqFdb_pu,
                            _IQmpy(obj->filteredIdFdb_pu, obj->torqueFactor_F) + obj->flux_Wb),
                            obj->torqueFactor_I);
}

void EstTrqTf_UpdateParams(EstTrqTfType *obj, float Ld_mH, float Lq_mH, float flux_Wb)
{
    obj->torqueFactor_F = _IQ((Ld_mH - Lq_mH) * obj->IBaseFactor);
    obj->flux_Wb        = flux_Wb;
}
