/*
 * ControlFuncs.c
 *
 *  Created on: 20 авг. 2023 г.
 *      Author: on4ip
 */
#include "ControlFuncs.h"
/*current offsets form ADC channels*/
float offset_Ia=0;
float offset_Ib=0;
float offset_Ic=0;


void FAST_loop(SYSTEMobj_st *SYSTEMobj)
{
	/*Get HW protection state*/
	if(GET_HWfault_state(SYSTEMobj->DRIVE->drive_ID))
	{
		SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 1;
	}
	if(!SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED)
	{
		offset_Ia += SYSTEMobj->DRIVE->currentABC.current_Ia * SYSTEMobj->DRIVE->Tsample_fast;
		offset_Ib += SYSTEMobj->DRIVE->currentABC.current_Ib * SYSTEMobj->DRIVE->Tsample_fast;
		offset_Ic += SYSTEMobj->DRIVE->currentABC.current_Ic * SYSTEMobj->DRIVE->Tsample_fast;
	}
	SYSTEMobj->DRIVE->currentABC.current_Ia	= SYSTEMobj->IO->current_A - offset_Ia;
	SYSTEMobj->DRIVE->currentABC.current_Ib	= SYSTEMobj->IO->current_B - offset_Ib;
	SYSTEMobj->DRIVE->currentABC.current_Ic	= SYSTEMobj->IO->current_C - offset_Ic;
	SYSTEMobj->DRIVE->voltage_DC			= SYSTEMobj->IO->voltage_DC;
	/*Process FOC control*/
	fast_loop_control(SYSTEMobj->DRIVE);
	/*Calculate shaft torque*/
	SYSTEMobj->DRIVE->torque_calc = SYSTEMobj->DRIVE->currentPark.current_Iq * SYSTEMobj->DRIVE->motor_data.Kt;
	/*Process Electrical Position Observer*/
	SYSTEMobj->OBSERVER_El->fullMode_ena = 0; /*Only speed calculation */
	speed_observer(SYSTEMobj->OBSERVER_El);
	SYSTEMobj->DRIVE->ElSpeed = SYSTEMobj->OBSERVER_El->W;
	SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->OBSERVER_El->Teta_rad;// + SYSTEMobj->OBSERVER_El->teta_correction;
	//if(SYSTEMobj->DRIVE->ElAngle >=toPI)	SYSTEMobj->DRIVE->ElAngle 	=	SYSTEMobj->DRIVE->ElAngle -toPI;
	//if(SYSTEMobj->DRIVE->ElAngle <0.f)	SYSTEMobj->DRIVE->ElAngle 	=	SYSTEMobj->DRIVE->ElAngle +toPI;
	/*Process Mechanical Position Observer */
	/*Observer dynamic correction in speed function*/
	SYSTEMobj->OBSERVER_M->refresh_ena = 1;
	SYSTEMobj->OBSERVER_M->fullMode_ena = 1; /*Full mode observer */
	SYSTEMobj->OBSERVER_M->Ml_izm = SYSTEMobj->DRIVE->torque_calc;
	SYSTEMobj->OBSERVER_M->L0 = mod(SYSTEMobj->OBSERVER_M->W)*-0.01f+0.93f;
	if(SYSTEMobj->OBSERVER_M->L0 < 0.9f ) SYSTEMobj->OBSERVER_M->L0 = 0.90f;
	speed_observer(SYSTEMobj->OBSERVER_M);
	SYSTEMobj->DRIVE->speed_Fdb = SYSTEMobj->OBSERVER_M->W;

	if(SYSTEMobj->DRIVE->CMD.cmd.bit_RST)	/*Clear fault control*/
	{
		CLEAR_HWfault(SYSTEMobj->DRIVE->drive_ID);
		SYSTEMobj->DRIVE->ERRORS.errors_word = 0;
		SYSTEMobj->DRIVE->CMD.cmd.bit_RST = 0;
		SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 0;
	}

	if(SYSTEMobj->DRIVE->CMD.cmd.bit_ON && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
	{
		SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 1; //Enable Drive
	}else
	{
		SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 0; //Disable Drive
	}
	/*Calculate PWM reference values*/
	float inv_dc = 1.0f / (SYSTEMobj->DRIVE->voltage_DC * VoltageNormalisation_coeff);
	SYSTEMobj->PWM->Ualfa = DRIVE.voltage_Valpha_ref*inv_dc;
	SYSTEMobj->PWM->Ubeta = DRIVE.voltage_Vbeta_ref*inv_dc;
	SYSTEMobj->PWM->Uo = 0 * inv_dc;
	SYSTEMobj->PWM->Trpd_pwm = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID);
	PWM_proc(SYSTEMobj->PWM);
	/*PWM module control*/
	//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH1N, LL_TIM_OCMODE_FORCED_ACTIVE);
	//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH2N, LL_TIM_OCMODE_FORCED_ACTIVE);
	//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH3N, LL_TIM_OCMODE_FORCED_ACTIVE);
	if(SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
	{
		PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,(uint16_t)SYSTEMobj->PWM->Ta,(uint16_t)SYSTEMobj->PWM->Tb,(uint16_t)SYSTEMobj->PWM->Tc);
		PWM_OUT_ENABLE(SYSTEMobj->DRIVE->drive_ID);
	}else
	{
		uint16_t HALF_period = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID)>>1;
		PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,HALF_period,HALF_period,HALF_period);
		PWM_OUT_DISABLE(SYSTEMobj->DRIVE->drive_ID);
	}
}
/*Rate limiter function for Theta */
static inline  void Rate_limiter(float *Rated_value, float Ref_value, float Up_rate, float Down_rate,float Tsample)
{
	const float Down = Tsample*Down_rate;
	const float Up = Tsample*Up_rate;
	if((Ref_value - *Rated_value) >=PI)
	{
		*Rated_value = (*Rated_value+toPI);
	}
	if((Ref_value - *Rated_value)<=-PI)
	{
		*Rated_value = (*Rated_value-toPI);
	}
	if (Ref_value >(*Rated_value + Up))
	{
	*Rated_value += Up;
	}
	else if (Ref_value <(*Rated_value - Down))
	{
	*Rated_value -= Down;
	}
	else *Rated_value = Ref_value;
}
void SLOW_loop(SYSTEMobj_st *SYSTEMobj)
{

	slow_loop_control(SYSTEMobj->DRIVE);
}