/*
 * detect_overload.c
 *
 *  Created on: 15 дек. 2020 г.
 *      Author: star
 */
#include <adc_tools.h>
#include <detect_overload.h>
#include <edrk_main.h>
#include <params_motor.h>
#include <params_pwm24.h>

#include "IQmathLib.h"

DETECT_OVERLOAD out_I_over_1_6 = DETECT_OVERLOAD_DEFAULTS;

#define CALLS_IN_PWM_INT 2  //Количество прерываний за период ШИМ (1 или 2)

void init_detect_overloads(void) {
    out_I_over_1_6.level_overload = _IQmpy(I_OUT_NOMINAL_IQ, _IQ(1.6));
    out_I_over_1_6.time_over_tics = (long) 15 * FREQ_PWM * CALLS_IN_PWM_INT;
    out_I_over_1_6.time_latch_tics = (long) 45 * FREQ_PWM * CALLS_IN_PWM_INT;
    out_I_over_1_6.tics_counter = 0;
    out_I_over_1_6.overload_detected = 0;

}

int calc_detect_overload(DETECT_OVERLOAD *v) {
    if (v->val > v->level_overload) {
        v->tics_counter += 1;
        if (v->tics_counter > v->time_over_tics) { v->tics_counter = v->time_over_tics;}
    } else {
        if (v->tics_counter > 0) { v->tics_counter -= 1; }
        else {v->tics_counter = 0;}
        if (v->overload_detected && v->tics_counter == 0) {
            v->overload_detected = 0;
        }
    }
    if (v->tics_counter >= v->time_over_tics) {
        v->overload_detected = 1;
        v->tics_counter = v->time_latch_tics;
    }
    return v->overload_detected;
}

#define LIMIT_DETECT_LEVEL 15938355 //95%

void check_all_power_limits() {
    _iq level_I_nominal = 0;

    edrk.power_limit.bits.limit_by_temper = edrk.temper_limit_koeffs.code_status;

    level_I_nominal = _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_Izad_rmp);
    if ((filter.iqIm_1 > level_I_nominal) || (filter.iqIm_1 > level_I_nominal) ||
            out_I_over_1_6.overload_detected) {
        edrk.power_limit.bits.limit_Iout = 1;
    } else {
        edrk.power_limit.bits.limit_Iout = 0;
    }

//    edrk.power_limit.bits.limit_UOM =

    if ((filter.PowerScalar + edrk.iq_power_kw_another_bs) >
                _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_limit_power_zad_rmp)
        // Данный способ для скалярного управления, для FOC, возможно, нужна векторная мощность.
        ) {
        edrk.power_limit.bits.limit_from_SVU = 1;
    } else {
        edrk.power_limit.bits.limit_from_SVU = 0;
    }

}