CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/Linux_HAL_Essentials/RCInput_UART/pic_firmware.asm
Views: 1799
1
#include <p16f690.inc>
2
3
list p=16f690
4
5
__config _INTRC_OSC_NOCLKOUT & _WDT_OFF & _PWRTE_ON & _MCLRE_OFF & _CP_OFF & _CPD_OFF & _BOR_ON & _IESO_OFF & _FCMEN_OFF
6
7
;
8
; This program will read until eight pwm inputs and will send its values
9
; through serial port at 115200bps.
10
;
11
;
12
; +5V -| VDD VSS |- GND
13
; input 6 -| RA5 RA0 |- input 1
14
; input 5 -| RA4 RA1 |- input 2
15
; input 4 -| RA3 RA2 |- input 3
16
; output 4 -| RC5 RC0 |- n. c.
17
; output 3 -| RC4 RC1 |- status led
18
; output 2 -| RC3 RC2 |- output 1
19
; n.c. -| RC6 RB4 |- input 8
20
; n.c. -| RC7 RB5 |- rx
21
; tx* -| RB7 RB6 |- input 7
22
;
23
;
24
; * In order to keep IO voltage at 3.3V use a voltage divider on TX.
25
;
26
; PS: outputs are not covered by this program yet.
27
;
28
29
; The INPUT_NUM is attached to input_mask and the
30
; interruption. Do not change this value without
31
; changing the interruption and mask.
32
#define INPUT_NUM 8
33
#define MAGIC 0x55AA
34
35
cblock 0x020
36
; input_value, input_rise and input_mirror
37
; are at the bank's beginning to make easy
38
; the exchange of data between them.
39
input_value:(INPUT_NUM * 2)
40
input_previous
41
input_current
42
input_changed
43
input_mask
44
input_updated
45
sent_update
46
endc
47
48
cblock 0x72
49
save_w
50
save_status
51
save_fsr
52
timer_l
53
timer_h
54
count
55
tmr1if_count
56
endc
57
58
cblock 0x0a0
59
; see input_value
60
input_rise:(INPUT_NUM * 2)
61
endc
62
63
cblock 0x120
64
; see input_value
65
input_mirror:(INPUT_NUM * 2)
66
endc
67
68
; Reset Vector
69
org 0x0000
70
nop
71
nop
72
nop
73
goto Main
74
75
; Interrupts Vector
76
org 0x0004
77
; save the current context
78
movwf save_w
79
swapf STATUS, w
80
clrf STATUS
81
movwf save_status
82
movf FSR, w
83
movwf save_fsr
84
85
; it's a input event
86
btfss INTCON, RABIF
87
goto IntInputDone
88
89
; capture timer 1
90
movf TMR1H, w
91
movwf timer_h
92
movf TMR1L, w
93
movwf timer_l
94
movf TMR1H, w
95
subwf timer_h, w
96
btfsc STATUS, Z
97
goto IntTMR1Done
98
movf TMR1H, w
99
movwf timer_h
100
movf TMR1L, w
101
movwf timer_l
102
IntTMR1Done:
103
104
; capture the current input state
105
movf PORTA, w
106
btfsc PORTB, RB6
107
iorlw (1<<6)
108
btfsc PORTB, RB4
109
iorlw (1<<7)
110
111
bcf INTCON, RABIF
112
113
; identify the changed inputs
114
movwf input_current
115
xorwf input_previous, w
116
movwf input_changed
117
movf input_current, w
118
movwf input_previous
119
120
; prepare for loop
121
movlw .1
122
movwf input_mask
123
movlw input_rise
124
movwf FSR
125
goto IntInputLoop
126
127
IntInputNext:
128
incf FSR, f
129
130
IntInputNextHalf:
131
; ... and ensure bank 0 selection
132
incf FSR, f
133
bsf FSR, 7
134
135
; done if no more channels
136
bcf STATUS, C
137
rlf input_mask, f
138
btfsc STATUS, C
139
goto IntInputDone
140
141
IntInputLoop:
142
; skip if channel not changed
143
movf input_mask, w
144
andwf input_changed, w
145
btfsc STATUS, Z
146
goto IntInputNext
147
148
; check if it is raising or falling
149
andwf input_current, w
150
btfss STATUS, Z
151
goto IntInputRise
152
153
; calculate and move the lsb to input_value
154
movf INDF, w
155
subwf timer_l, w
156
bcf FSR, 7
157
movwf INDF
158
159
; calculate and move the msb to input_value
160
bsf FSR, 7
161
incf FSR, f
162
movf INDF, w
163
btfss STATUS, C
164
incf INDF, w
165
subwf timer_h, w
166
bcf FSR, 7
167
movwf INDF
168
169
goto IntInputNextHalf
170
171
IntInputRise:
172
; tell main loop we have all channels
173
btfsc input_mask, 1
174
incf input_updated, f
175
176
; save the rise instant
177
movf timer_l, w
178
movwf INDF
179
incf FSR, f
180
movf timer_h, w
181
movwf INDF
182
goto IntInputNextHalf
183
184
IntInputDone:
185
186
; restore the previous context
187
movf save_fsr, w
188
movwf FSR
189
swapf save_status, w
190
movwf STATUS
191
swapf save_w, f
192
swapf save_w, w
193
retfie
194
195
Main:
196
; bank 0
197
clrf STATUS
198
199
clrf PORTA
200
clrf PORTB
201
clrf PORTC
202
203
; bank 1
204
bsf STATUS, RP0
205
206
movlw (b'111'<<IRCF0)
207
movwf OSCCON ; set internal oscillator frequency to 8 MHz
208
209
movlw (0<<NOT_RABPU)
210
movwf OPTION_REG
211
212
; ,------ input 6
213
; |,----- input 5
214
; ||,---- input 4
215
; |||,--- input 3
216
; ||||,-- input 2
217
; |||||,- input 1
218
movlw b'11111111'
219
movwf TRISA ; set PORTA as input
220
movwf WPUA ; enable weak pull-up
221
movwf IOCA ; enable interrupt-on-change
222
223
; ,-------- tx
224
; |,------- input 6
225
; ||,------ rx
226
; |||,----- input 7
227
movlw b'01111111'
228
movwf TRISB
229
230
; ,------ output 4
231
; |,----- output 3
232
; ||,---- output 2
233
; |||,--- output 1
234
; ||||,-- status
235
; |||||,- n. c.
236
movlw b'11000001'
237
movwf TRISC
238
239
movlw (1<<TXEN|1<<BRGH)
240
movwf TXSTA ; enable async transmitter and select high baud rate
241
242
movlw (1<<BRG16)
243
movwf BAUDCTL ; 16-bit baud rate generator
244
245
movlw .16 ; use value 16 for SPBRG:SPBRGH to get 115200bps baud rate
246
movwf SPBRG
247
clrf SPBRGH
248
249
; Bank 2
250
bcf STATUS, RP0
251
bsf STATUS, RP1
252
253
clrf ANSEL
254
clrf ANSELH
255
256
; ,------- input 6
257
; | ,----- input 7
258
movlw b'01010000'
259
movwf WPUB
260
movwf IOCB
261
262
; bank 0
263
clrf STATUS
264
265
; clear the first two banks
266
movlw 0x020
267
call ClearBank
268
movlw 0x0A0
269
call ClearBank
270
271
movlw (1<<SPEN|1<<CREN)
272
movwf RCSTA
273
274
movlw (1<<T1CKPS0|1<<TMR1ON)
275
movwf T1CON ; enable timer and set frequency to 1MHz (=8Mhz/4/2)
276
277
movlw (1<<GIE|1<<PEIE|1<<RABIE)
278
movwf INTCON
279
280
MainLoopNoUpdate:
281
; toggle status led if idle for more than INPUT_NUM * 2000 usec
282
movf timer_h, w
283
subwf TMR1H, w
284
sublw high(.2000 * INPUT_NUM)
285
btfsc STATUS, C
286
goto MainLoop
287
; toggle status led at each 8 timer1 overflows, i.e., about 0.52 sec
288
btfss PIR1, TMR1IF
289
goto MainLoop
290
bcf PIR1, TMR1IF
291
incf tmr1if_count, f
292
movlw H'07'
293
andwf tmr1if_count, w
294
btfss STATUS, Z
295
goto MainLoop
296
; toggle status led
297
movf PORTC, w
298
xorlw (1<<RC1)
299
movwf PORTC
300
MainLoop:
301
; loop if no updates
302
movf sent_update, w
303
subwf input_updated, w
304
btfsc STATUS, Z
305
goto MainLoopNoUpdate
306
307
; prepare for mirroring
308
addwf sent_update, f
309
movlw (INPUT_NUM * 2)
310
movwf count
311
movlw input_value
312
movwf FSR
313
314
MirrorLoop:
315
movf INDF, w
316
bsf STATUS, IRP
317
movwf INDF
318
bcf STATUS, IRP
319
incf FSR, f
320
decfsz count, f
321
goto MirrorLoop
322
323
; restart if there was update while mirroring
324
movf input_updated, w
325
subwf sent_update, w
326
btfss STATUS, Z
327
goto MainLoop
328
329
; finally send the captured values
330
; send a magic number to sync
331
movlw low(MAGIC)
332
call Send
333
movlw high(MAGIC)
334
call Send
335
336
; prepare for send loop
337
movlw (INPUT_NUM * 2)
338
movwf count
339
movlw low(input_mirror)
340
bsf STATUS, IRP
341
movwf FSR
342
343
SendLoop:
344
movf INDF, w
345
call Send
346
incf FSR, f
347
decfsz count, f
348
goto SendLoop
349
350
bcf STATUS, IRP
351
352
; set led on
353
bcf PORTC, RC1
354
355
goto MainLoop
356
357
Send:
358
btfss PIR1, TXIF
359
goto $ - 1
360
movwf TXREG
361
return
362
363
ClearBank:
364
; clears the gpr bank pointed by w plus irp,
365
; but preserves the common 16 bytes.
366
367
; ensure the beginning of the bank
368
andlw b'10100000'
369
movwf FSR
370
movlw .80
371
movwf count
372
373
ClearBankLoop:
374
clrf INDF
375
incf FSR, f
376
decfsz count, f
377
goto ClearBankLoop
378
return
379
380
end
381
382
383