/*
 * vector.h
 *
 *  Created on: 20 сент. 2023 г.
 *      Author: seklyuts
 */



#ifndef SRC_VECTOR_H_
#define SRC_VECTOR_H_

#include "pi.h"
#include "pwm_init.h"

#define sng(x)      ((x)?((x>0)?1:(-1)):(0))
#define max(a,b)    ((a>b)?a:b)
#define min(a,b)    ((a<b)?a:b)
#define rsh(x,n) ( (x>=0)?(x>>n):( -((-x)>>n) ))
#define abs(x) (x>0?x:-x)
#define saturate(x,a,b) ((x>b)?b:((x<a)?a:x))
#define FRAC16(x) ((int16_t)((x) < 1 ? ((x) >= -1 ? ((int16_t)((x)*0x8000)) : ((int16_t)0x8000)) : ((int16_t)0x7FFF)))

//#define FRAC16(x) ((int16_t)((x)*0x7FFF))

#define FRAC32(x) ((int32_t)((x) < 1 ? ((x) >= -1 ? ((int32_t)((x)*0x80000000)) : ((int32_t)0x80000000)) : ((int32_t)0x7FFFFFFF)))


#define SQRT3                   1.7320508
#define ONE_DIV_SQRT3           FRAC16(1/SQRT3)
#define HALF_SQRT3              FRAC16(SQRT3/2)
#define ANGLE_PI_DIVIDE_2_F16   16384
#define SIN_INDEX_SHIFT         6
#define SIN_LENGTH_TABLE        256

#define VOLT_MAX_PROC           94.0
#define VOLT_MAX_FACTOR         VOLT_MAX_PROC/100.0
#define PI_REG_I_PROPOR         30.0
#define PI_REG_I_INTEGR         0.04
#define CURRENT_MAX             40.0 //A
#define ZERO_LVL                0.00001f

typedef struct
{
  float q;
  float d;
}Tvector2dq;


typedef struct
{
  float Alfa;
  float Beta;
}Tvector2ph;

typedef struct
{
  float a;
  float b;
  float c;
}Tvector3abc;

typedef struct
{
  int16_t Angle;
  float sin;
  float cos;
}TvectorSinCos;

typedef struct
{
  float CurrentLimit;  //A
  PI_CONTROLLER piIq;
  PI_CONTROLLER piId;
}TvectorCurrentLoop;

typedef enum
{
  OffMode,             //Мотор остановлен
  StepMode,             //
  StayMode,
  CurrentRegTune,
  VectorModeU,
  VectorModeI
}TMode;




int16_t vector_mcsinPIxLUT(int16_t Value, int16_t *psinTable);
void vector_inversion(void);
void vector_klark_park(uint16_t SectorOn, int16_t CurrentA, int16_t CurrentB, int16_t CurrentC);
void vectorInitCurrLoop(void);
void vectorControl(int16_t CurrentA, int16_t CurrentB, int16_t CurrentC, int16_t sdfmUdc);
void vectorFault(void);
float my_sqrtf(float x);
void vectorResCurrLoop(void);


#endif /* SRC_VECTOR_H_ */