310 lines
8.7 KiB
C
310 lines
8.7 KiB
C
/*
|
|
* Academic License - for use in teaching, academic research, and meeting
|
|
* course requirements at degree granting institutions only. Not for
|
|
* government, commercial, or other organizational use.
|
|
* File: nco.c
|
|
*
|
|
* MATLAB Coder version : 24.1
|
|
* C/C++ source code generated on : 2025-04-09 02:18:34
|
|
*/
|
|
|
|
/* Include Files */
|
|
#include "nco.h"
|
|
#include "rt_nonfinite.h"
|
|
#include "rt_nonfinite.h"
|
|
#include <math.h>
|
|
|
|
/* Function Declarations */
|
|
static double rt_roundd_snf(double u);
|
|
|
|
/* Function Definitions */
|
|
/*
|
|
* Arguments : double u
|
|
* Return Type : double
|
|
*/
|
|
static double rt_roundd_snf(double u)
|
|
{
|
|
double y;
|
|
if (fabs(u) < 4.503599627370496E+15) {
|
|
if (u >= 0.5) {
|
|
y = floor(u + 0.5);
|
|
} else if (u > -0.5) {
|
|
y = u * 0.0;
|
|
} else {
|
|
y = ceil(u - 0.5);
|
|
}
|
|
} else {
|
|
y = u;
|
|
}
|
|
return y;
|
|
}
|
|
|
|
/*
|
|
* Arguments : double fcw
|
|
* double ptw
|
|
* double clr
|
|
* double acc
|
|
* double *my_c
|
|
* double *my_s
|
|
* double *acc_o
|
|
* Return Type : void
|
|
*/
|
|
void nco(double fcw, double ptw, double clr, double acc, double *my_c,
|
|
double *my_s, double *acc_o)
|
|
{
|
|
static const int res_cos[96] = {
|
|
39, 39, 39, 39, 39, 39, 39, 39, 39,
|
|
38, 38, 38, 38, 37, 37, 37, 36, 36,
|
|
35, 35, 35, 34, 34, 33, 33, 32, 31,
|
|
31, 30, 30, 29, 28, 0, 79, 158, 237,
|
|
315, 394, 472, 550, 628, 705, 782, 858, 934,
|
|
1009, 1084, 1158, 1231, 1304, 1376, 1446, 1517, 1586,
|
|
1654, 1721, 1787, 1852, 1916, 1979, 2041, 2101, 2161,
|
|
2218, 262143, 262065, 261828, 261434, 260882, 260173, 259307, 258285,
|
|
257107, 255774, 254288, 252648, 250856, 248913, 246820, 244578, 242189,
|
|
239655, 236975, 234153, 231190, 228088, 224848, 221473, 217965, 214325,
|
|
210556, 206660, 202640, 198498, 194236, 189857};
|
|
static const int res_sin[96] = {
|
|
0, 1, 2, 3, 4, 5, 6, 7, 8,
|
|
9, 10, 11, 12, 13, 14, 15, 16, 16,
|
|
17, 18, 19, 20, 21, 22, 22, 23, 24,
|
|
25, 25, 26, 27, 28, 3217, 3216, 3213, 3208,
|
|
3202, 3193, 3182, 3170, 3155, 3139, 3121, 3101, 3079,
|
|
3055, 3029, 3002, 2972, 2941, 2908, 2874, 2837, 2799,
|
|
2759, 2718, 2675, 2630, 2584, 2536, 2487, 2436, 2384,
|
|
2330, 0, 6433, 12863, 19284, 25695, 32089, 38464, 44817,
|
|
51142, 57436, 63696, 69917, 76096, 82230, 88314, 94344, 100318,
|
|
106232, 112081, 117863, 123574, 129210, 134769, 140246, 145639, 150945,
|
|
156159, 161279, 166302, 171225, 176045, 180759};
|
|
double cn;
|
|
double longp;
|
|
double pha;
|
|
double pha_n;
|
|
double r;
|
|
double x;
|
|
double y;
|
|
int acc_o_tmp;
|
|
/* function
|
|
* [my_c,my_s,acc_o,pha,qua,longp,longn,segp,segn,pha_p,pha_n,cp,sp,cn,sn]=nco(fcw,ptw,clr,acc)
|
|
*/
|
|
x = acc + fcw;
|
|
if (rtIsNaN(x) || rtIsInf(x)) {
|
|
pha = rtNaN;
|
|
} else if (x == 0.0) {
|
|
pha = 0.0;
|
|
} else {
|
|
pha = fmod(x, 2.81474976710656E+14);
|
|
if (pha == 0.0) {
|
|
pha = 0.0;
|
|
} else if (x < 0.0) {
|
|
pha += 2.81474976710656E+14;
|
|
}
|
|
}
|
|
acc_o_tmp = (clr == 0.0);
|
|
*acc_o = (double)acc_o_tmp * pha;
|
|
/* res = coef(); */
|
|
/* pha = round(acc_o/2^29) + ptw*2^3; */
|
|
/* qua = floor(mod(pha/2^16,8)); */
|
|
/* seg = floor(mod((pha/2^11),32)); */
|
|
/* segp = seg + 1; */
|
|
/* segn = 32 - seg; */
|
|
/* pha = mod(pha,2^11); */
|
|
/* */
|
|
/* c2_list_hex=["3ffff","3ffb1","3fec4","3fd3a","3fb12","3f84d","3f4eb","3f0ed","3ec53","3e71e","3e150","3dae8","3d3e8","3cc51","3c424","3bb62","3b20d","3a827","39daf","392a9",...
|
|
*/
|
|
/* "38716","37af8","36e50","36121","3536d","34535","3367c","32744","31790","30762","2f6bc","2e5a1"];
|
|
*/
|
|
/* c1_list_hex=["3ffff","3ffb1","3fec4","3fd3a","3fb12","3f84d","3f4eb","3f0ed","3ec53","3e71e","3e150","3dae8","3d3e8","3cc51","3c424","3bb62","3b20d","3a827","39daf","392a9",...
|
|
*/
|
|
/* "38716","37af8","36e50","36121","3536d","34535","3367c","32744","31790","30762","2f6bc","2e5a1"];
|
|
*/
|
|
/* */
|
|
pha = floor((double)acc_o_tmp * acc / 5.36870912E+8) + ptw * 8.0;
|
|
x = pha / 65536.0;
|
|
if (rtIsNaN(x) || rtIsInf(x)) {
|
|
r = rtNaN;
|
|
} else if (x == 0.0) {
|
|
r = 0.0;
|
|
} else {
|
|
r = fmod(x, 8.0);
|
|
if (r == 0.0) {
|
|
r = 0.0;
|
|
} else if (x < 0.0) {
|
|
r += 8.0;
|
|
}
|
|
}
|
|
if (rtIsNaN(pha) || rtIsInf(pha)) {
|
|
longp = rtNaN;
|
|
} else if (pha == 0.0) {
|
|
longp = 0.0;
|
|
} else {
|
|
longp = fmod(pha, 65536.0);
|
|
if (longp == 0.0) {
|
|
longp = 0.0;
|
|
} else if (pha < 0.0) {
|
|
longp += 65536.0;
|
|
}
|
|
}
|
|
/* 低16位 */
|
|
x = longp / 2048.0;
|
|
if (rtIsNaN(x)) {
|
|
pha = rtNaN;
|
|
} else if (x == 0.0) {
|
|
pha = 0.0;
|
|
} else {
|
|
pha = fmod(x, 32.0);
|
|
if (pha == 0.0) {
|
|
pha = 0.0;
|
|
} else if (x < 0.0) {
|
|
pha += 32.0;
|
|
}
|
|
}
|
|
y = floor(pha);
|
|
x = (65535.0 - longp) / 2048.0;
|
|
if (rtIsNaN(x)) {
|
|
pha = rtNaN;
|
|
} else if (x == 0.0) {
|
|
pha = 0.0;
|
|
} else {
|
|
pha = fmod(x, 32.0);
|
|
if (pha == 0.0) {
|
|
pha = 0.0;
|
|
} else if (x < 0.0) {
|
|
pha += 32.0;
|
|
}
|
|
}
|
|
x = floor(pha);
|
|
if (rtIsNaN(longp)) {
|
|
pha = rtNaN;
|
|
pha_n = rtNaN;
|
|
} else {
|
|
if (longp == 0.0) {
|
|
pha = 0.0;
|
|
} else {
|
|
pha = fmod(longp, 2048.0);
|
|
if (pha == 0.0) {
|
|
pha = 0.0;
|
|
} else if (longp < 0.0) {
|
|
pha += 2048.0;
|
|
}
|
|
}
|
|
if (65535.0 - longp == 0.0) {
|
|
pha_n = 0.0;
|
|
} else {
|
|
pha_n = fmod(65535.0 - longp, 2048.0);
|
|
if (pha_n == 0.0) {
|
|
pha_n = 0.0;
|
|
} else if (65535.0 - longp < 0.0) {
|
|
pha_n += 2048.0;
|
|
}
|
|
}
|
|
}
|
|
/* cp = round((round((round(res_cos(segp,1)*pha_p/2^11) +
|
|
* res_cos(segp,2))*pha_p/2^10) + res_cos(segp,3))/2^3); */
|
|
/* sp = round((round((round(res_sin(segp,1)*pha_p/2^11) +
|
|
* res_sin(segp,2))*pha_p/2^10) + res_sin(segp,3))/2^3); */
|
|
/* cn = round((round((round(res_cos(segn,1)*pha_n/2^11) +
|
|
* res_cos(segn,2))*pha_n/2^10) + res_cos(segn,3))/2^3); */
|
|
/* sn = round((round((round(res_sin(segn,1)*pha_n/2^11) +
|
|
* res_sin(segn,2))*pha_n/2^10) + res_sin(segn,3))/2^3); */
|
|
longp = rt_roundd_snf(
|
|
((double)res_cos[(int)(y + 1.0) + 63] -
|
|
rt_roundd_snf(
|
|
(rt_roundd_snf((double)res_cos[(int)(y + 1.0) - 1] * pha / 2048.0) +
|
|
(double)res_cos[(int)(y + 1.0) + 31]) *
|
|
pha / 1024.0)) /
|
|
8.0);
|
|
cn = rt_roundd_snf(
|
|
((double)res_cos[(int)(x + 1.0) + 63] -
|
|
rt_roundd_snf((rt_roundd_snf((double)res_cos[(int)(x + 1.0) - 1] *
|
|
pha_n / 2048.0) +
|
|
(double)res_cos[(int)(x + 1.0) + 31]) *
|
|
pha_n / 1024.0)) /
|
|
8.0);
|
|
*my_s = rt_roundd_snf(
|
|
((double)res_sin[(int)(y + 1.0) + 63] +
|
|
rt_roundd_snf(
|
|
((double)res_sin[(int)(y + 1.0) + 31] -
|
|
rt_roundd_snf((double)res_sin[(int)(y + 1.0) - 1] * pha / 2048.0)) *
|
|
pha / 1024.0)) /
|
|
8.0);
|
|
pha = rt_roundd_snf(
|
|
((double)res_sin[(int)(x + 1.0) + 63] +
|
|
rt_roundd_snf(((double)res_sin[(int)(x + 1.0) + 31] -
|
|
rt_roundd_snf((double)res_sin[(int)(x + 1.0) - 1] *
|
|
pha_n / 2048.0)) *
|
|
pha_n / 1024.0)) /
|
|
8.0);
|
|
switch ((int)floor(r)) {
|
|
case 0:
|
|
*my_c = longp;
|
|
break;
|
|
case 1:
|
|
*my_c = pha;
|
|
*my_s = cn;
|
|
break;
|
|
case 2:
|
|
*my_c = -*my_s;
|
|
*my_s = longp;
|
|
break;
|
|
case 3:
|
|
*my_c = -cn;
|
|
*my_s = pha;
|
|
break;
|
|
case 4:
|
|
*my_c = -longp;
|
|
*my_s = -*my_s;
|
|
break;
|
|
case 5:
|
|
*my_c = -pha;
|
|
*my_s = -cn;
|
|
break;
|
|
case 6:
|
|
*my_c = *my_s;
|
|
*my_s = -longp;
|
|
break;
|
|
case 7:
|
|
*my_c = cn;
|
|
*my_s = -pha;
|
|
break;
|
|
default:
|
|
*my_c = 0.0;
|
|
*my_s = 0.0;
|
|
break;
|
|
}
|
|
if (*my_c >= 32768.0) {
|
|
*my_c = 32767.0;
|
|
} else if (*my_c < -32768.0) {
|
|
*my_c = -32768.0;
|
|
}
|
|
if (*my_s >= 32768.0) {
|
|
*my_s = 32767.0;
|
|
} else if (*my_s < -32768.0) {
|
|
*my_s = -32768.0;
|
|
}
|
|
/* result.pha = round(acc/2^29 + ptw*2^3); */
|
|
}
|
|
|
|
/*
|
|
* Arguments : void
|
|
* Return Type : void
|
|
*/
|
|
void nco_initialize(void)
|
|
{
|
|
rt_InitInfAndNaN();
|
|
}
|
|
|
|
/*
|
|
* Arguments : void
|
|
* Return Type : void
|
|
*/
|
|
void nco_terminate(void)
|
|
{
|
|
}
|
|
|
|
/*
|
|
* File trailer for nco.c
|
|
*
|
|
* [EOF]
|
|
*/
|