/*
 * regul_power.c
 *
 *  Created on: 16 нояб. 2020 г.
 *      Author: star
 */
#include "IQmathLib.h"
#include "regul_power.h"

#include <edrk_main.h>
#include <master_slave.h>
#include <params_motor.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include "math.h"

#include "mathlib.h"

#define TIME_RMP_SLOW 20.0    //sec
#define TIME_RMP_FAST 20.0    //sec

POWER power = POWER_DEFAULTS;

void init_Pvect(void) {
    power.pidP.Ref = 0;
    power.pidP.Kp = _IQ(1);
    power.pidP.Ki = _IQ(0.1);
    power.pidP.Kc = _IQ(0.5);
    power.pidP.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
    power.pidP.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);

    power.Pzad_rmp = 0;
    power.koef_fast = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_FAST / (float)FREQ_PWM);
    power.koef_slow = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM);
    power.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
    power.Pnominal = _IQ(P_NOMINAL * 1000.0 / NORMA_ACP / NORMA_ACP);
}

#pragma CODE_SECTION(vector_power,".fast_run");
_iq vector_power(_iq Pzad, _iq P_measured, int n_alg, unsigned int master,
                 _iq Iq_measured, _iq Iq_limit, _iq *Iq_zad, int reset)
{
    static int prev_n_alg = 0;
    _iq Iq_out = 0;
	_iq koef_rmp = 0;
    if (n_alg != ALG_MODE_FOC_POWER || !edrk.Go || master != MODE_MASTER || reset) {
        power.pidP.Ui = Iq_measured;
        power.pidP.Out = Iq_measured;
        if (reset) { power.Pzad_rmp = 0; }
    }
    if (n_alg == ALG_MODE_FOC_OBOROTS) {
        Pzad = power.Pnominal;
    }
    if (master == MODE_SLAVE) {
    	power.Pzad_rmp = P_measured;
    	return *Iq_zad;
    }

    //Для перехода из режима поддержания оборотов в режим поддержания мощности,
    //Т.к. в режиме поддержания оборотов всегда задаётся максимальная допустимая мощность
    if (n_alg == ALG_MODE_FOC_POWER && prev_n_alg != ALG_MODE_FOC_POWER) {
    	power.Pzad_rmp = P_measured;
    }

    if((_IQabs(power.Pzad_rmp) <= _IQabs(Pzad)) &&
        (((Pzad >= 0) && (power.Pzad_rmp >= 0)) || ((Pzad < 0) && (power.Pzad_rmp < 0))))
    {
        koef_rmp = power.koef_slow;
    }
    else
    {
        koef_rmp = power.koef_fast;
    }
    power.Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, power.Pzad_rmp, Pzad);

    power.pidP.OutMax = Iq_limit;
    power.pidP.OutMin = -Iq_limit;
    power.pidP.Ref = power.Pzad_rmp;
    power.pidP.Fdb = P_measured;
    power.pidP.calc(&power.pidP);
    Iq_out = power.pidP.Out;
    Iq_out = _IQsat(Iq_out, Iq_limit, -Iq_limit);
    *Iq_zad = Iq_out;

    prev_n_alg = n_alg;

    return Iq_out;
}