86 lines
2.3 KiB
C
86 lines
2.3 KiB
C
/*
|
|
* skvt.c
|
|
*
|
|
* Created on: 11 îêò. 2023 ã.
|
|
* Author: seklyuts
|
|
*/
|
|
#include "f28x_project.h"
|
|
#include "skvt.h"
|
|
|
|
/*
|
|
1) t = arctan(sin/cos)
|
|
2) t = PI/2-arctan(cos/sin)
|
|
3) t = PI/2+arctan(-cos/sin)
|
|
4) t = PI-arctan(sin/-cos)
|
|
5) t = -PI+arctan(-sin/-cos)
|
|
6) t = -PI/2-arctan(-cos/-sin)
|
|
7) t = -PI/2+arctan(cos/-sin)
|
|
8) t = -arctan(-sin/cos)
|
|
|
|
y= -0.0915 x^2 + 0.3434x - 0.0015
|
|
*/
|
|
#define A_p -FRAC16(0.0915)
|
|
#define B_p FRAC16(0.3434)
|
|
#define C_p -FRAC16(0.0015)
|
|
|
|
int16_t AngleSector;
|
|
|
|
int16_t mult_r(int16_t x, int16_t y) { return (int16_t) (((int32_t) (x) * (y)) >> 15); }
|
|
|
|
int16_t Atan_approximation(int16_t x)
|
|
{
|
|
int16_t xx,y;
|
|
|
|
if (x<0) y = FRAC16(0.25); //x=32767;//it will be so if (sin == cos), because (x<<15)/x = (x * 2^8)/x = 2^8 = 0x8000 = -32768;
|
|
else {
|
|
if(x <= C_p) y = 0;
|
|
else {
|
|
xx=mult_r(x,x);
|
|
y = mult_r(xx,A_p) + mult_r(x,B_p) + C_p;
|
|
}
|
|
}
|
|
return (y);
|
|
}
|
|
|
|
|
|
int16_t Atan(int16_t rSinA, int16_t rCosA)
|
|
{
|
|
int16_t theta_actual_el_1 = 0;
|
|
int16_t MrCosA, MrSinA;
|
|
|
|
if(rSinA >= 0) {
|
|
if(rCosA >= 0) { //0 < 0.5
|
|
if(rSinA < rCosA) { AngleSector=1;
|
|
theta_actual_el_1 = Atan_approximation(((int32_t)rSinA<<15)/rCosA);
|
|
} else { AngleSector = 2;
|
|
theta_actual_el_1 = FRAC16(0.5)-Atan_approximation(((int32_t)rCosA<<15)/rSinA);
|
|
}
|
|
} else { //0.5 < 1
|
|
MrCosA = abs(rCosA);
|
|
if(rSinA < MrCosA) { AngleSector = 4;
|
|
theta_actual_el_1 = FRAC16(1)-Atan_approximation(((int32_t)rSinA<<15)/MrCosA);
|
|
} else { AngleSector = 3;
|
|
theta_actual_el_1 = FRAC16(0.5)+Atan_approximation(((int32_t)MrCosA<<15)/rSinA);
|
|
}
|
|
}
|
|
} else {
|
|
MrSinA = abs(rSinA);
|
|
if(rCosA >= 0) { //0 > -0.5
|
|
if(MrSinA < rCosA) { AngleSector = 8;
|
|
theta_actual_el_1 = -Atan_approximation(((int32_t)MrSinA<<15)/rCosA);
|
|
} else { AngleSector = 7;
|
|
theta_actual_el_1 = -FRAC16(0.5)+Atan_approximation(((int32_t)rCosA<<15)/MrSinA);
|
|
}
|
|
} else { //-0.5 > -1
|
|
MrCosA = abs(rCosA);
|
|
if(MrSinA < MrCosA) { AngleSector = 5;
|
|
theta_actual_el_1 = -FRAC16(1)+Atan_approximation(((int32_t)MrSinA<<15)/MrCosA);
|
|
} else { AngleSector = 6;
|
|
theta_actual_el_1 = -FRAC16(0.5)-Atan_approximation(((int32_t)MrCosA<<15)/MrSinA);
|
|
}
|
|
}
|
|
}
|
|
|
|
return(theta_actual_el_1);
|
|
}
|