/**********************************************************************
 * © 2014 Microchip Technology Inc.
 *
 * SOFTWARE LICENSE AGREEMENT:
 * Microchip Technology Incorporated ("Microchip") retains all ownership and
 * intellectual property rights in the code accompanying this message and in all
 * derivatives hereto.  You may use this code, and any derivatives created by
 * any person or entity by or on your behalf, exclusively with Microchip's
 * proprietary products.  Your acceptance and/or use of this code constitutes
 * agreement to the terms and conditions of this notice.
 *
 * CODE ACCOMPANYING THIS MESSAGE IS SUPPLIED BY MICROCHIP "AS IS".  NO
 * WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE APPLY TO THIS CODE, ITS INTERACTION WITH MICROCHIP'S
 * PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION.
 *
 * YOU ACKNOWLEDGE AND AGREE THAT, IN NO EVENT, SHALL MICROCHIP BE LIABLE, WHETHER
 * IN CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE OR BREACH OF STATUTORY DUTY),
 * STRICT LIABILITY, INDEMNITY, CONTRIBUTION, OR OTHERWISE, FOR ANY INDIRECT, SPECIAL,
 * PUNITIVE, EXEMPLARY, INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, FOR COST OR EXPENSE OF
 * ANY KIND WHATSOEVER RELATED TO THE CODE, HOWSOEVER CAUSED, EVEN IF MICROCHIP HAS BEEN
 * ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE.  TO THE FULLEST EXTENT
 * ALLOWABLE BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO
 * THIS CODE, SHALL NOT EXCEED THE PRICE YOU PAID DIRECTLY TO MICROCHIP SPECIFICALLY TO
 * HAVE THIS CODE DEVELOPED.
 *
 * You agree that you are solely responsible for testing the code and
 * determining its suitability.  Microchip has no obligation to modify, test,
 * certify, or support the code.
 *
 ******************************************************************************/
/**********************************************************************
 *      Code Description                                               *
 *                                                                     *
 *  This file implements a slide mode observer. This observer is used  *
 *  to estimate rotor position and speed. Rotor position, Theta, is    *
 *  then compensated from phase delays introduced by the filters       *
 *                                                                     *
 **********************************************************************/

#include "general.h"
#include "smcpos.h"
#include <libq.h>
#include <stdint.h>

void SMC_Position_Estimation(SMC *s)
{

    uint16_t saved_corcon = CORCON;
    CORCON = (CORCON | _CORCON_SATA_MASK | _CORCON_SATB_MASK | _CORCON_ACCSAT_MASK);

    CalcEstI();

    CalcIError();

    /* Sliding control calculator */

    if (_Q15abs(s->iAlphaError) < s->maxSMCError)
    {
        /* s->Zalpha = (s->Kslide * s->IalphaError) / s->MaxSMCError
        /* If we are in the linear range of the slide mode controller,
        /* then correction factor Zalpha will be proportional to the
        /* error (Ialpha - EstIalpha) and slide mode gain, Kslide.*/
        CalcZalpha();
    }
    else if (s->iAlphaError > 0)
    {
        s->zAlpha = s->kSlide;
    }
    else
    {
        s->zAlpha = -s->kSlide;
    }
    if (_Q15abs(s->iBetaError) < s->maxSMCError)
    {
        /* s->Zbeta = (s->Kslide * s->IbetaError) / s->MaxSMCError
        /* If we are in the linear range of the slide mode controller,
        /* then correction factor Zbeta will be proportional to the
        /* error (Ibeta - EstIbeta) and slide mode gain, Kslide. */
        CalcZbeta();
    }
    else if (s->iBetaError > 0)
    {
        s->zBeta = s->kSlide;
    }
    else
    {
        s->zBeta = -s->kSlide;
    }
    /* Sliding control filter -> back EMF calculator
    /* s->Ealpha = s->Ealpha + s->Kslf * s->Zalpha -
    /*						   s->Kslf * s->Ealpha
    /* s->Ebeta = s->Ebeta + s->Kslf * s->Zbeta -
    /*						 s->Kslf * s->Ebeta
    /* s->EalphaFinal = s->EalphaFinal + s->KslfFinal * s->Ealpha
    /*								   - s->KslfFinal * s->EalphaFinal
    /* s->EbetaFinal = s->EbetaFinal + s->KslfFinal * s->Ebeta
    /*								 - s->KslfFinal * s->EbetaFinal */
    CalcBEMF();

    /* Rotor angle calculator -> Theta = atan(-EalphaFinal,EbetaFinal) */

    s->theta = atan2CORDIC(-s->eAlphaFinal, s->eBetaFinal);

    accumTheta += s->theta - prevTheta;
    prevTheta = s->theta;

    accumThetaCnt++;
    if (accumThetaCnt == IRP_PERCALC)
    {
        s->omega = accumTheta;
        accumThetaCnt = 0;
        accumTheta = 0;
    }
    /*                    Q15(Omega) * 60
    /* Speed RPMs = -----------------------------
    /*               SpeedLoopTime * Motor Poles
    /* For example:
    /*    Omega = 0.5
    /*    SpeedLoopTime = 0.001
    /*    Motor Poles (pole pairs * 2) = 10
    /* Then:
    /*    Speed in RPMs is 3,000 RPMs

    /* s->OmegaFltred = s->OmegaFltred + FilterCoeff * s->Omega
    /*								   - FilterCoeff * s->OmegaFltred */

    CalcOmegaFltred();

    /* Adaptive filter coefficients calculation
    /* Cutoff frequency is defined as 2*_PI*electrical RPS
    /*
    /* 		Wc = 2*_PI*Fc.
    /* 		Kslf = Tpwm*2*_PI*Fc
    /*
    /* Fc is the cutoff frequency of our filter. We want the cutoff frequency
    /* be the frequency of the drive currents and voltages of the motor, which
    /* is the electrical revolutions per second, or eRPS.
    /*
    /* 		Fc = eRPS = RPM * Pole Pairs / 60
    /*
    /* Kslf is then calculated based on user parameters as follows:
    /* First of all, we have the following equation for RPMS:
    /*
    /* 		RPM = (Q15(Omega) * 60) / (SpeedLoopTime * Motor Poles)
    /*		Let us use: Motor Poles = Pole Pairs * 2
    /*		eRPS = RPM * Pole Pairs / 60), or
    /*		eRPS = (Q15(Omega) * 60 * Pole Pairs) / (SpeedLoopTime * Pole Pairs * 2 * 60)
    /*	Simplifying eRPS
    /*		eRPS = Q15(Omega) / (SpeedLoopTime * 2)
    /*	Using this equation to calculate Kslf
    /*		Kslf = Tpwm*2*_PI*Q15(Omega) / (SpeedLoopTime * 2)
    /*	Using diIrpPerCalc = SpeedLoopTime / Tpwm
    /*		Kslf = Tpwm*2*Q15(Omega)*_PI / (diIrpPerCalc * Tpwm * 2)
    /*	Simplifying:
    /*		Kslf = Q15(Omega)*_PI/diIrpPerCalc
    /*
    /* We use a second filter to get a cleaner signal, with the same coefficient
    /*
    /* 		Kslf = KslfFinal = Q15(Omega)*_PI/diIrpPerCalc
    /*
    /* What this allows us at the end is a fixed phase delay for theta compensation
    /* in all speed range, since cutoff frequency is changing as the motor speeds up.
    /*
    /* Phase delay: Since cutoff frequency is the same as the input frequency, we can
    /* define phase delay as being constant of -45 DEG per filter. This is because
    /* the equation to calculate phase shift of this low pass filter is
    /* arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG.
    /* A total of -90 DEG after the two filters implemented (Kslf and KslfFinal).*/

    s->kslf = s->kslfFinal = FracMpy(s->omegaFltred, Q15(_PI / IRP_PERCALC));

    /* Since filter coefficients are dynamic, we need to make sure we have a minimum
    /* so we define the lowest operation speed as the lowest filter coefficient */

    if (s->kslf < Q15(OMEGA0 * _PI / IRP_PERCALC))
    {
        s->kslf = Q15(OMEGA0 * _PI / IRP_PERCALC);
        s->kslfFinal = Q15(OMEGA0 * _PI / IRP_PERCALC);
    }
    s->thetaOffset = CONSTANT_PHASE_SHIFT;
    s->theta = s->theta + s->thetaOffset;


    CORCON = saved_corcon;
    return;
}

void SMCInit(SMC *s)
{
    /*                R * Ts
    /* Fsmopos = 1 - --------
    /*                  L
    /*            Ts
    /* Gsmopos = ----
    /*            L
    /* Ts = Sampling Period. If sampling at PWM, Ts = 50 us
    /* R = Phase Resistance. If not provided by motor datasheet,
    /*     measure phase to phase resistance with multimeter, and
    /*     divide over two to get phase resistance. If 4 Ohms are
    /*     measured from phase to phase, then R = 2 Ohms
    /* L = Phase inductance. If not provided by motor datasheet,
    /*     measure phase to phase inductance with multimeter, and
    /*     divide over two to get phase inductance. If 2 mH are
    /*     measured from phase to phase, then L = 1 mH */

    if (Q15(PHASE_RES * LOOPTIME_IN_SEC) > Q15(PHASE_IND))
    {
        s->fsmopos = Q15(0.0);
    }
    else
    {
        s->fsmopos = Q15(1 - PHASE_RES * LOOPTIME_IN_SEC / PHASE_IND);
    }
    if (Q15(LOOPTIME_IN_SEC) > Q15(PHASE_IND))
    {
        s->gsmopos = Q15(0.99999);
    }
    else
    {
        s->gsmopos = Q15(LOOPTIME_IN_SEC / PHASE_IND * UMAX / IPEAK);
    }
    s->kSlide = Q15(SMC_GAIN);
    s->maxSMCError = Q15(MAX_LINEAR_SMC);
    s->filtOmCoef = Q15(OMEGA0 * _PI / IRP_PERCALC); /* Cutoff frequency for omega filter */
    /* is minimum omega, or OMEGA0 */

    return;
}

