blob: 8784f2c647bbe593a16ccaf1c04db514dd7d62fb (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
|
// Angle Normalize
//
// angle_normalize - normalize angle to [-PI, PI]
//
// y[t] = fmod( x[t], 2*PI ), adjusted to [-PI, PI]
//
// X_n - 1
// Y_n - 1
// C_n - 0
#include <math.h>
#include "ts.h"
ts_parameter_t const ts_angle_normalize_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 0, .params_size = 0 };
uint_t ts_angle_normalize_offset( ts_constant_t const *c )
{
( void )c;
return ( 0 );
}
int ts_angle_normalize(
fp_t * const *y, uint_t y_n,
fp_t const * const *x, uint_t x_n,
uint_t n, ts_constant_t const *c
)
{
TS_PROLOGUE( ts_angle_normalize_p );
(void)c;
for( uint_t i = 0; i < n; ++i )
{
fp_t r = fmod( x[0][i], 2.0 * PI );
if( r > PI )
r -= 2.0 * PI;
if( r < -PI )
r += 2.0 * PI;
y[0][i] = r;
}
return 0;
}
|