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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
#include "rotation.h"
#include <cmath>
void rotate_soft(int8_t *soft, int size, phase_t phase, bool iqswap)
{
int8_t tmp;
// Scale down to avoid overflows
for (int i = 0; i < size; i++)
if (soft[i] == -128)
soft[i] = -127;
// Swap I & Q if requested
if (iqswap)
{
for (int i = 0; i < size; i += 2)
{
int8_t x = soft[i + 1];
soft[i + 1] = soft[i];
soft[i] = x;
}
}
// Rotate phase
switch (phase)
{
case PHASE_0:
// Nothing to do!
break;
case PHASE_90:
// Rotate 90 deg
for (; size > 0; size -= 2)
{
tmp = *soft;
*soft = *(soft + 1);
*(soft + 1) = -tmp;
soft += 2;
}
break;
case PHASE_180:
// Rotate 180
for (; size > 0; size--)
{
*soft = -*soft;
soft++;
}
break;
case PHASE_270:
// Rotate 270
for (; size > 0; size -= 2)
{
tmp = *soft;
*soft = -*(soft + 1);
*(soft + 1) = tmp;
soft += 2;
}
break;
default:
// This should never happen
break;
}
}
int8_t clamp(float x)
{
if (x < -128.0)
return -127;
if (x > 127.0)
return 127;
return x;
}
void rotate_soft_arbitrary(int8_t *soft, int size, float phase)
{
float shift_real = cos(phase);
float shift_imag = sin(phase);
int8_t vr = 0;
int8_t vi = 0;
for (int i = 0; i < size / 2; i++)
{
vr = clamp((soft[i * 2 + 0] * shift_real) - (soft[i * 2 + 1] * shift_imag));
vi = clamp((soft[i * 2 + 1] * shift_real) + (soft[i * 2 + 0] * shift_imag));
soft[i * 2 + 0] = vr;
soft[i * 2 + 1] = vi;
}
}
|
