-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmeasureChannels.p
More file actions
197 lines (138 loc) · 7.98 KB
/
Copy pathmeasureChannels.p
File metadata and controls
197 lines (138 loc) · 7.98 KB
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
// Accurately measure duty cycle of PWM signal from 4 different channels and store it in
// a memory location accessible from an external program
// Generate PWMs based on the data at those memory locations
// Written by Miguel Rufino
.origin 0 // offset of start of program in PRU memory
.entrypoint START // program entry point used by the debugger
#define INS_PER_US 200
#define INS_PER_LOOP 2
#define PRU0_R31_VEC_VALID 32
#define PRU_EVTOUT_0 3
#define PRU_EVTOUT_1 4
// r1 : iteration counter
// r0 : measurement storage location
// r3 : channel 1 timer
// r4 : channel 2 timer
// r5 : channel 3 timer
// r6 : channel 4 timer
// r11 : channel 1 pwm out
// r12 : channel 2 pwm out
// r13 : channel 3 pwm out
// r14 : channel 4 pwm out
// r16 : signal period
START:
// Read number of samples to read
MOV r0, 0x00000000 //load the memory location, number of samples
LBBO r1, r0, 0, 4 //load the value into memory - keep r1
MOV r0, 0x00000008 // going to write the result to this address
LBBO r16, r0, 16, 4 // load signal period (1/frequency) 51Hz(?) for Turnigy
MOV r3, 1 // r3 will store the echo pulse width for channel 1
SBBO r3, r0, 0, 4 // initialize to 0
MOV r4, 1 // r4 will store the echo pulse width for channel 2
SBBO r4, r0, 4, 4 // initialize to 0
MOV r5, 1 // r5 will store the echo pulse width for channel 3
SBBO r5, r0, 8, 4 // initialize to 0
MOV r6, 1 // r6 will store the echo pulse width for channel 4
SBBO r6, r0, 12, 4 // initialize to 0
MOV r11, 100000
MOV r12, 100000
MOV r13, 100000
MOV r14, 100000
MOV r31.b0, 0
MEASURE:
CHANNEL1:
QBBS ADDCHAN1, r31.t5 // If r31.t5 is high then count (measuring echo pulse width)
QBEQ NOPULSE1, r3, 0 // if pulse has not started do nothing
SBBO r3, r0, 0, 4 // if pulse is done (not high) store to memory
MOV r3, -1 // reset channel counter ( -1 since ADDCHAN adds 1 after)
//MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_1 // generate interrupt
ADDCHAN1:
ADD r3, r3, 1 // Increment channel counter
NOPULSE1:
CHANNEL2:
QBBS ADDCHAN2, r31.t3 // If r31.t3 is high then count (measuring echo pulse width)
QBEQ NOPULSE2, r4, 0 // if pulse has not started do nothing
SBBO r4, r0, 4, 4 // if pulse is done (not high) store to memory
MOV r4, -1 // reset channel counter ( -1 since ADDCHAN adds 1 after)
//MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_1 // generate interrupt
ADDCHAN2:
ADD r4, r4, 1 // Increment channel counter
NOPULSE2:
CHANNEL3:
QBBS ADDCHAN3, r31.t1 // If r31.t1 is high then count (measuring echo pulse width)
QBEQ NOPULSE3, r5, 0 // if pulse has not started do nothing
SBBO r5, r0, 8, 4 // if pulse is done (not high) store to memory
MOV r5, -1 // reset channel counter ( -1 since ADDCHAN adds 1 after)
//MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_1 // generate interrupt
ADDCHAN3:
ADD r5, r5, 1 // Increment channel counter
NOPULSE3:
CHANNEL4:
QBBS ADDCHAN4, r31.t2 // If r31.t2 is high then count (measuring echo pulse width)
QBEQ NOPULSE4, r6, 0 // if pulse has not started do nothing
SBBO r6, r0, 12, 4 // if pulse is done (not high) store to memory
MOV r6, -1 // reset channel counter ( -1 since ADDCHAN adds 1 after)
MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0 // generate interrupt
ADDCHAN4:
ADD r6, r6, 1 // Increment channel counter
NOPULSE4:
GENERATE:
GEN1:
QBNE NOCHANGE1,r11,0 // if timer isn't 0 skip ( signal doesn't need to be changed)
// else flip signal
QBBS MAKELOW1,r30.t15 // if high make low
MAKEHIGH1: // if low
SET r30.t15 // make high
LBBO r11,r0,20,4 // load latest duty cycle for channel
QBA NOCHANGE1
MAKELOW1:
CLR r30.t15 // make low
LBBO r11,r0,20,4 // load latest duty cycle for channel
SUB r11,r16,r11 // set timer = period - duty cycle
NOCHANGE1:
SUB r11,r11,1 // decrement counter
GEN2:
QBNE NOCHANGE2,r12,0 // if timer isn't 0 skip ( signal doesn't need to be changed)
// else flip signal
QBBS MAKELOW2,r30.t14 // if high make low
MAKEHIGH2: // if low
SET r30.t14 // make high
LBBO r12,r0,24,4 // load latest duty cycle for channel
QBA NOCHANGE2
MAKELOW2:
CLR r30.t14 // make low
LBBO r12,r0,24,4 // load latest duty cycle for channel
SUB r12,r16,r12 // set timer = period - duty cycle
NOCHANGE2:
SUB r12,r12,1 // decrement counter
GEN3:
QBNE NOCHANGE3,r13,0 // if timer isn't 0 skip ( signal doesn't need to be changed)
// else flip signal
QBBS MAKELOW3,r30.t7 // if high make low
MAKEHIGH3: // if low
SET r30.t7 // make high
LBBO r13,r0,28,4 // load latest duty cycle for channel
QBA NOCHANGE3
MAKELOW3:
CLR r30.t7 // make low
LBBO r13,r0,28,4 // load latest duty cycle for channel
SUB r13,r16,r13 // set timer = period - duty cycle
NOCHANGE3:
SUB r13,r13,1 // decrement counter
GEN4:
QBNE NOCHANGE4,r14,0 // if timer isn't 0 skip ( signal doesn't need to be changed)
// else flip signal
QBBS MAKELOW4,r30.t0 // if high make low
MAKEHIGH4: // if low
SET r30.t0 // make high
LBBO r14,r0,32,4 // load latest duty cycle for channel
QBA NOCHANGE4
MAKELOW4:
CLR r30.t0 // make low
LBBO r14,r0,32,4 // load latest duty cycle for channel
SUB r14,r16,r14 // set timer = period - duty cycle
NOCHANGE4:
SUB r14,r14,1 // decrement counter
ITERATIONS:
//MOV R31.b0, PRU0_R31_VEC_VALID | PRU_EVTOUT_0 // generate interrupt
QBA MEASURE