/*
 * 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 "afwsc.h"

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

#define DAMPING_FACTOR (4)

/*******************************************************************************
 * the typedefs
 ******************************************************************************/

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

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

void Afwsc_Init(AfwscType *obj, Afwsc_ParamsType *pParams)
{
    obj->speedRef = 0;

    /* Calculate PID parameters */
    _iq   Ki_series   = _IQ(pParams->spdFilterPole_rps / (DAMPING_FACTOR * DAMPING_FACTOR) * pParams->runFreq_kHz / 1000);
    float K           = 1.5 * pParams->polePairs * pParams->rotorFlux_Wb / pParams->inertia_kgm2;
    _iq   Kp_series   = _IQ(pParams->spdFilterPole_rps / (K * DAMPING_FACTOR)
                        * 2 * MATH_PI * pParams->fBase_Hz / pParams->polePairs / pParams->IBase_A);
    _iq   Kp_parallel = Kp_series;
    _iq   Ki_parallel = _IQmpy(Ki_series, Kp_series);

    Pid_Init(&obj->pidSpd, Kp_parallel, Ki_parallel, 0, _IQ(0), _IQ(pParams->maxCurrent_A / pParams->IBase_A));

    Ramp_Init(&obj->spdRamp, _IQ(0.000005));

    Fw_Init(&obj->fwObj, _IQ(pParams->minId_A / pParams->IBase_A));
}

void Afwsc_Run(AfwscType *obj)
{
    /* Field weakening */
    obj->fwObj.refValue   = obj->VsRef;
    obj->fwObj.fbackValue = obj->Vs;
    Fw_Run(&obj->fwObj);
    obj->IdRef = obj->fwObj.output;

    /* Ramp for the reference */
    obj->spdRamp.target = obj->speedRef;
    Ramp_Calc(&obj->spdRamp);
    /* PID for speed */
    obj->pidSpd.ref = obj->spdRamp.output;
    obj->pidSpd.fdb = obj->speed;
    Pid_Run(&obj->pidSpd);
    /* PID output is iq */
    obj->IqRef = obj->pidSpd.output;
}

void Afwsc_Reset(AfwscType *obj)
{
    obj->fwObj.output = 0;
    obj->pidSpd.Ui     = 0;
    ;
    obj->IdRef = 0;
    obj->IqRef = 0;
}
