-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsps.nolol
96 lines (82 loc) · 3.65 KB
/
sps.nolol
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
90
91
92
93
94
95
96
include "std/logic"
// enums
define SPS_MODE_NONE=0
define SPS_MODE_MONO=1
define SPS_MODE_QUAD=4
macro sps_calc(X, Y, Z, R1, R2, R3, R3)<CX1, CX2, CX3, CX4, CY1, CY2, CY3, CY4, CZ1, CZ2, CZ3, CZ4> line
X=R1/CX1+R2/CX2+R3/CX3+R4/CX4; Y=R1/CY1+R2/CY2+R3/CY3+R4/CY4; Z=R1/CZ1+R2/CZ2+R3/CZ3+R4/CZ4
end
// setup quad reciver
macro sps_quad_init(Receiver1_TargetMessage, Receiver2_TargetMessage, Receiver3_TargetMessage, Receiver4_TargetMessage)<TargetMessage1, TargetMessage2, TargetMessage3, TargetMessage4> block
Receiver1_TargetMessage=TargetMessage1
Receiver2_TargetMessage=TargetMessage2
Receiver3_TargetMessage=TargetMessage3
Receiver4_TargetMessage=TargetMessage4
end
// measure and calc from a quad reciver setup
macro sps_quad(X, Y, Z, Receiver1_SignalStrength, Receiver2_SignalStrength, Receiver3_SignalStrength, Receiver4_SignalStrength)<MaxRange> block
R1=MaxRange-Receiver1_SignalStrength; R1*=R1
R2=MaxRange-Receiver2_SignalStrength; R2*=R2
R3=MaxRange-Receiver3_SignalStrength; R3*=R3
R4=MaxRange-Receiver4_SignalStrength; R4*=R4
sps_calc(X,Y,Z,R1,R2,R3,R3)
end
// measure and calc from a mono reciver setup
macro sps_mono(X, Y, Z, Receiver_TargetMessage, Receiver_SignalStrength)<MaxRange, TargetMessage1, TargetMessage2, TargetMessage3, TargetMessage4> block
// next=measure1; Receiver1_TargetMessage=TargetMessage1; goto calc
// measure1> R1=MaxRange-Receiver1_SignalStrength; R1*=R1; next=measure2; Receiver1_TargetMessage=TargetMessage2; goto calc
// measure2> R2=MaxRange-Receiver1_SignalStrength; R2*=R2; next=measure3; Receiver1_TargetMessage=TargetMessage3; goto calc
// measure3> R3=MaxRange-Receiver1_SignalStrength; R3*=R3; next=measure4; Receiver1_TargetMessage=TargetMessage4; goto calc
// measure4> R4=MaxRange-Receiver1_SignalStrength; R4*=R4; next=measure1; Receiver1_TargetMessage=TargetMessage1; goto calc
// calc>
Receiver_TargetMessage=TargetMessage1
Receiver_SignalStrength=0
logic_wait(Receiver_SignalStrength)
R1=MaxRange-Receiver_SignalStrength
R1*=R1
Receiver_TargetMessage=TargetMessage2
Receiver_SignalStrength=0
logic_wait(Receiver_SignalStrength)
R2=MaxRange-Receiver_SignalStrength
R2*=R2
Receiver_TargetMessage=TargetMessage3
Receiver_SignalStrength=0
logic_wait(Receiver_SignalStrength)
R3=MaxRange-Receiver_SignalStrength
R3*=R3
Receiver_TargetMessage=TargetMessage4
Receiver_SignalStrength=0
logic_wait(Receiver_SignalStrength)
R4=MaxRange-Receiver_SignalStrength
R4*=R4
// Receiver_TargetMessage=TargetMessage1 $
// R1=MaxRange-Receiver_SignalStrength
// R1*=R1
// Receiver_TargetMessage=TargetMessage2 $
// R2=MaxRange-Receiver_SignalStrength
// R2*=R2
// Receiver_TargetMessage=TargetMessage3 $
// R3=MaxRange-Receiver_SignalStrength
// R3*=R3
// Receiver_TargetMessage=TargetMessage4 $
// R4=MaxRange-Receiver_SignalStrength
// R4*=R4
// Receiver_TargetMessage=TargetMessage1 $
sps_calc(X,Y,Z,R1,R2,R3,R3)
end
macro sps_full(X, Y, Z, Mode, Receiver_1_Signal_Strength, Receiver_1_Target_Message, Receiver_2_Signal_Strength, Receiver_2_Target_Message, Receiver_3_Signal_Strength, Receiver_3_Target_Message, Receiver_4_Signal_Strength, Receiver_4_Target_Message) block
if Mode!=SPS_MODE_NONE then
sps_load_consts()
if Mode==SPS_MODE_QUAD then
sps_quad_init(Receiver_1_Signal_Strength,Receiver_2_TargetMessage,Receiver_3_TargetMessage,Receiver_4_TargetMessage)
end
while 1 do
if Mode==SPS_MODE_MONO then
sps_mono(X,Y,Z,Receiver_1_Target_Message,Receiver_1_Signal_Strength)
else if Mode==SPS_MODE_QUAD then
sps_quad(X,Y,Z,Receiver_1_Signal_Strength,Receiver_2_Signal_Strength,Receiver_3_Signal_Strength,Receiver_4_Signal_Strength)
end
SPS1_T=Clock_Time
end
end
end