-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy path0006.html
More file actions
559 lines (545 loc) · 36.5 KB
/
0006.html
File metadata and controls
559 lines (545 loc) · 36.5 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
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<link rel="alternate" type="application/atom+xml" title="Atom Feed" href="feed.xml">
<link rel="stylesheet" type="text/css" href="style.css">
<title>How not to intercept a PWM signal - Lars' Website</title>
</head>
<body>
<header>
<nav>
<h1><a href="index.html">Lars' website</a></h1>
<a href="archive.html" title="archive">Archive</a>
<a href="feed.xml" title="feed">Feed</a>
</nav>
</header>
<article>
<header>
<h2>How not to intercept a PWM signal</h2>
<div>First published: <time>2022-06-12</time></div>
</header>
<section>
<p>
Recently I was put in charge of a computerified 1:8 model RC car that is used in workshops to teach autonomous driving and machine learning concepts.
But not only can this little car drive autonomously, it can also be driven with a traditional radio control!
(Which workshop participants are sometimes way more interested in than autonomous driving.)
</p>
<p>
In this article I'll focus on one specific quirk I investigated and fixed: every now and then the car hat sudden spurts of twitching for no apparent reason and calmed down after a while.
By 'twitching' I mean it would shake its steerable front wheels and give small pulses with it's electrical motor.
While the shaking wasn't very severe and didn't worsen the driving performance too much, it was still quite audible and would look especially strange when the car was supposed to be standing still.
By 'no apparent reason' I mean that it could happen in any driving mode: when steered via the radio control, during autonomous driving via software and also while parking.
</p>
</section>
<section>
<h3>The System</h3>
<p>
Before we get into the debugging process, I'll take a moment to elaborate on the system architecture of the car.
While based on a off-the-shelf 1:8 electric model car with radio receiver, steering servo, brushless motor, gearing and suspension, a bunch of electronics have been added:
</p>
<ul>
<li>In the center of it all sits a Mini-ITX based computer running Linux.</li>
<li>Spread around the car are a few Arduino Micro boards.</li>
<li>There are also a LIDAR, cameras, and some more sensors that aren't relevant here.</li>
<li>Everything is connected to the Linux computer via USB.</li>
<li>One of the Arduinos is hooked up to the radio receiver as well as the steering servo and motor driver.</li>
<li>PD2 and PD3 are used to read the radio signal, PD6 and PC7 are used to control the motor and steering.</li>
<li>Even in remote-control-mode the radio signal makes a round-trip through the Linux PC and back to the Arduino.</li>
</ul>
<p>
And here's a diagram because I love <a href="https://en.wikipedia.org/wiki/Box_Drawing" target="_blank">box drawing characters</a>:
</p>
<code id="figure0">
<pre>
╔══════════════╗ ╔══════════════╗ ╔═══════════╗ ╔════════════════╗
║ Linux PC ║ ║ USB Hub ║ ║ Arduino ║ ║ Radio Receiver ║
╟──────────────╢ ╟──────────────╢ ╟───────────╢ ╟────────────────╢
║ ║ ║ ║ ║ PD2╫──PWM──╫Throttle ║
║ ║ ║ ║ ║ ║ ║ ║
║ ║ ║ ║ ║ PD3╟──PWM──╫Steering ║
║ ║ ║ ║ ║ ║ ╚════════════════╝
║ ║ ║ ║ ║ ║
║ ║ ║ ║ ║ ║ ╔════════════════╗
║ USB╫─────╫USB USB╫──UART──╫USB PC6╫──PWM──╢ Motor Driver ║
║ ║ ║ ║ ║ ║ ╚════════════════╝
║ ║ ║ ║ ║ ║
║ ║ ║ ║ ║ ║ ╔════════════════╗
║ ║ ║ ║ ║ PD7╫──PWM──╢ Steering Servo ║
║ ║ ║ ║ ╚═══════════╝ ╚════════════════╝
║ ║ ║ ║
║ ║ ║ ║ ╔═══════════╗
║ ║ ║ USB╫────────╢ ... ║
║ ║ ╚══════════════╝ ╚═══════════╝
║ ║
║ ║ ╔══════════════╗
║ USB╫─────╢ LIDAR ║
║ ║ ╚══════════════╝
║ ║
║ ║ ╔══════════════╗
║ USB╫─────╢ ... ║
╚══════════════╝ ╚══════════════╝
</pre>
<figcaption>Figure 0: System overview</figcaption>
</code>
</section>
<section>
<h3>Pulse Width Modulation</h3>
<p>
While we're at introductions, let's also quickly go over PWM, especially as it is used for RC cars.
A really useful tool to understand PWM is an oscilloscope (well, a logic analyzer might be enough), so let's pretend we have one:
</p>
<code id="figure1">
<pre>
╔═══════════════════════════════════════════════════════════════════════════════════════╗
║ LarTronix 1ms/div 4V/div 166.7Hz ║
╟───────────────────────────────────────────────────────────────────────────────────────╢
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · ├───────────┤ · · · · ├───────────┤ · ║
║· · · · · · · ·│· · · · · ·│· · · · · · · · · · · · · · · · · ·│· · · · · ·│· · · · · ·║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║Steering ) ────┤· · · · · ·├───────────────────────────────────┤· · · · · ·├───────────║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · ├───────────┤ · · · · ├───────────┤ · ║
║· · · · · · · ·│· · · · · ·│· · · · · · · · · · · · · · · · · ·│· · · · · ·│· · · · · ·║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║Throttle ) ────┤· · · · · ·├───────────────────────────────────┤· · · · · ·├───────────║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
╚═══════════════════════════════════════════════════════════════════════════════════════╝
</pre>
<figcaption>Figure 1: nice little PWM waveforms</figcaption>
</code>
<p>
What we have in fig. 1 is what you should see when you connect an oscilloscope to two of the PWM related pins on the Arduino.
Time goes from left to right, each grid cell being 0.001s, so the "high" part of the signal is 1.5ms long.
The shape of the signal repeats after 6ms, which is an exaggeration on my part to make it easier to visualize, the real "period" of the signal was 16.67ms.
Oftentimes with PWM, we want to know the "duty cycle" which is the length the signal was "high" relative to the length of the period.
With RC cars (and planes, drones, ...) a different convention is used: 1.5ms is defined to be "neutral" and this time is reduced or increased by up to 500µs to drive forward and backward or steer left right right.
So this is what we get when turning the radio controllers steering wheel all the way to the left, giving full throttle and watching pin PD3 and PD2 on the Arduino:
</p>
<code id="figure2">
<pre>
╔═══════════════════════════════════════════════════════════════════════════════════════╗
║ LarTronix 1ms/div 4V/div 166.7Hz ║
╟───────────────────────────────────────────────────────────────────────────────────────╢
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · ├───────┤ · · · · ├───────┤ · ║
║· · · · · · · ·│· · · ·│· · · · · · · · · · · · · · · · · · · ·│· · · ·│· · · · · · · ·║
║ · │ │ · · · · │ │ · ║
║ · │ │ · · · · │ │ · ║
║ · │ │ · · · · │ │ · ║
║Steering ) ────┤· · · ·├───────────────────────────────────────┤· · · ·├───────────────║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · ├───────────────┤ · · · ├───────────────┤ ║
║· · · · · · · ·│· · · · · · · ·│· · · · · · · · · · · · · · · ·│· · · · · · · ·│· · · ·║
║ · │ · │ · · · │ · │ ║
║ · │ · │ · · · │ · │ ║
║ · │ · │ · · · │ · │ ║
║Throttle ) ────┤· · · · · · · ·├───────────────────────────────┤· · · · · · · ·├───────║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
║ · · · · · · · · · · ║
╚═══════════════════════════════════════════════════════════════════════════════════════╝
</pre>
<figcaption>Figure 2: counterclockwise doughnuts!</figcaption>
</code>
<p>
The only thing that changes is the distance between the rising and falling edge, while the period remains unchanged.
Had we turned the steering wheel the other way, the edges would go further apart to 2ms and look the same as the throttle signal.
Of course many values in between these extremes are also possible to get smooth steering and throttle control.
</p>
</section>
<section>
<h3>Microcontrolling</h3>
<p>
How do we work with such a PWM signal?
In general, your average microcontroller offers a bunch of options to deal with PWM signals and the <a href="https://www.microchip.com/en-us/product/ATmega32U4" target="_blank">ATmega32U4</a> chip on the Arduino Micro is no different.
However, these options are bound to specific pins, so let's see what we can use on PD2 and PD3 as inputs.
</p>
<code id="figure3">
<pre>
┌────┐
╔═══╡USB ╞═══╗
OC4A | CLKO | ICP3 | PC7 ─╫13 └┬┬┬┬┘ 12╫─ PD6 | T1 | !OC4D | | ADC9
3.3V out ─╫ 11╫─ PB7 | OC0A | OC1C | PCINT7 | !RTS
AREF ─╫ 10╫─ PB6 | OC1B | OC4B | PCINT6 | ADC13
TDI | ADC7 | PF7 ─╫A0 ┌──┐ 9╫─ PB5 | OC1A | !OC4B | PCINT5 | ARC12
TDO | ADC6 | PF6 ─╫A1 └──┘ 8╫─ PB4 | | | PCINT4 | ADC11
TMS | ADC5 | PF5 ─╫A2 7╫─ PE6 | | | INT6 | AIN0
TCK | ADC4 | PF4 ─╫A3┌──────┐ 6╫─ PD7 | T0 | OC4D | | ADC10
ADC1 | PF1 ─╫A4│ATMEGA│ 5╫─ PC6 | OC3A | !OC4A | |
ADC0 | PF0 ─╫A5│ 32U4 │ 4╫─ PD4 | ICP1 | | | ADC8
n.c. ─╫ └──────┘ 3╫─ PD0 | SCL | OC0B | !INT0
n.c. ─╫ 2╫─ PD1 | SDA | | !INT1
5V out ─╫ ┌──┐ ╫─ GND
Reset ─╫ ╞══╡ ╫─ Reset
GND ─╫ └──┘ 0╫─ PD2 | RXD1 | | !INT2
Vin ─╫ oo ┌┴┴┐ 1╫─ PD3 | TXD1 | | !INT3
PCINT3 | PDO | PB3 ─╫ oo │()│ ╫─ PB0 | !SS | | PCINT0
PCINT1 | SCK | PB1 ─╫ oo └┬┬┘ ╫─ PB2 | PDI | MOSI | PCINT2
╚════════════╝
</pre>
<figcaption>Figure 3: Arduino Micro pinout</figcaption>
</code>
<p>
There are not many functions on these pins: <code>RX/TXD1</code> are for serial communications, which we don't need here.
The other option is much more promising: the external interrupts on these pins can be used to run some code whenever the logical level changes between high and low.
We will also need a way to get the current time while inside the interrupt, but Arduino already offers the <a href="https://www.arduino.cc/reference/en/language/functions/time/micros/" target="_blank"><code>micros()</code></a> function for that.
With interrupts we should be careful to do as little work as possible, because the ATmega32U4 does not support nested interrupts.
This means that while the program is inside an interrupt and the conditions for another, more important interrupt arise, the first one still has to complete before the next one can start.
</p>
<p>
So much for reading in PWM signals, how about outputting them?
For that we have to look at PC6 and PD7, which offer multiple "output compare" modes.
Output compare is perfect to create a PWM signal: there is a counter constantly going up towards some maximum value where it is reset to 0 and the output pin is set high.
Additionally there is a compare value.
When the counter reaches the compare value, the output pin is set low.
The result: a PWM waveform where we can move the falling edge by setting the compare value.
</p>
<p>
If we want to produce a 6ms long signal where the first 1.5ms are high and know that the counter runs at 16MHz, then the compare value should be 24000 and reset should happen at 96000 counts.
These values are a bit too large for this poor 8-bit chip, so we can use a prescaler to reduce the counter frequency and use smaller compare and reset values to produce the same timing.
But that's enough about microcontrollers for now, let's finally get back to the car!
</p>
</section>
<section>
<h3>Problem analysis</h3>
<p>
So here's the situation: we have the twitching car, signals going form the radio controller to the receiver, through the Arduino, through an USB cable to the Linux PC, through some convoluted software, back through the USB cable, through the Arduino, to the motor and servo.
</p>
<p>
That's a lot of potential failure points, where to start?
To get a better picture of how severe this problem is I started where I was pretty sure to find a bad signal: between the Arduino and the servo.
With a scope and a few wires I found this:
</p>
<code id="figure4">
<pre>
╔═══════════════════════════════════════════════════════════════════════════════════════╗
║ LarTronix 1ms/div 4V/div 166.7Hz Δt: 1.58ms ║
╟───────────────────────────────────────────────────────────────────────────────────────╢
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
║ · ├───────────┤ · · · · ├────────────┤ · ║
║· · · · · · · ·│· · · · · ·│· · · · · · · · · · · · · · · · · ·│· · · · · · │ · · · · ·║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║Steering ) ────┤· · · · · ·├───────────────────────────────────┤· · · · · · ├──────────║
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
╚═══════════════════════════════════════════════════════════════════════════════════════╝
</pre>
<figcaption>Figure 4: the HTML blink tag would have been perfect for this!</figcaption>
</code>
<p>
The car was supposed to be completely neutral, outputting an 1.5ms signal.
What I found was that this was true most of the time, but occasionally the signal was up to 80µs longer than it should be.
As expected, these incorrect timings exactly matched when the car was twitching around.
</p>
<p>
While I'm at it I might as well probe the input side.
Maybe there is a problem with the radio control or receiver.
</p>
<code id="figure5">
<pre>
╔═══════════════════════════════════════════════════════════════════════════════════════╗
║ LarTronix 1ms/div 4V/div 166.7Hz Δt: 1.5ms ║
╟───────────────────────────────────────────────────────────────────────────────────────╢
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
║ · ├───────────┤ · · · · ├───────────┤ · ║
║· · · · · · · ·│· · · · · ·│· · · · · · · · · · · · · · · · · ·│· · · · · ·│· · · · · ·║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║ · │ · │ · · · · │ · │ · ║
║Steering ) ────┤· · · · · ·├───────────────────────────────────┤· · · · · ·├───────────║
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
║ · · · · · · · ┆ · ┆ · ║
╚═══════════════════════════════════════════════════════════════════════════════════════╝
</pre>
<figcaption>Figure 5: nope</figcaption>
</code>
<p>
Then let's see what is going over this serial connection, maybe we find something suspicious there...
</p>
<code id="figure6">
<pre>
...
In: 0x5A 0x5A
Out: 0x5A 0x5A
In: 0x5A 0x5A
Out: 0x5A 0x5A
In: 0x5A 0x5A
Out: 0x5A 0x5A
In: 0x5A 0x5A
Out: 0x5A 0x5A
In: 0x5A 0x5A
Out: 0x5A 0x5A
In: 0x5A 0x5A
Out: 0x5A 0x5A
...
</pre>
<figcaption>Figure 6</figcaption>
</code>
<p>
Nothing to see here...
Other than the fact that apparently 90<sub>10</sub> is the neutral value, instead of 80<sub>16</sub>, as you might expect from a single byte (or y'know using a signed byte with 0 being neutral).
</p>
<p>
So it seems that the Arduino is getting the same input over and over again, but is somehow creating different outputs.
Time to dig into the sourcecode and find out out more.
</p>
</section>
<section>
<h3>Inside the heart of darkness</h3>
<p>
Let's try to figure out the path of the signal from serial to PWM, maybe there is something obvious in there.
</p>
<code id="listing0">
<pre>
<span>...</span>
<span>int16_t cmdSteering;</span>
<span>int16_t cmdDrive;</span>
<span>...</span>
<span>Servo servo_steering;</span>
<span>Servo servo_speed;</span>
<span>...</span>
<span>void setup() {</span>
<span> ...</span>
<span> servo_steering.attach(PIN_STEER_SERV);</span>
<span> servo_speed.attach(PIN_SPEED_CONTR);</span>
<span></span>
<span> servo_steering.write(90);</span>
<span> servo_speed.write(90);</span>
<span> ...</span>
<span>}</span>
<span>...</span>
<span>void update() {</span>
<span> ...</span>
<span> cmdDrive = Serial.read();</span>
<span> cmdSteering = Serial.read();</span>
<span> ...</span>
<span> servo_steering.write(cmdSteering);</span>
<span> servo_speed.write(cmdDrive);</span>
<span>}</span>
<span>...</span>
</pre>
<figcaption>Listing 0</figcaption>
</code>
<p>
I left out a lot here but this is ultimately it: the values are read in and passed to some instances of a class called Servo.
Even though these variables are global, nobody else is modifying them, at least on purpose.
It's hope that we're not dealing with some out-of-bounds write that messes with these values...
</p>
<p>
Okay, what is this Servo class?
Turns out it is a default <a href="https://github.com/arduino-libraries/Servo" target="_blank">Arduino library</a> for creating PWM signals on any GPIO pin.
Wait, any pin?
But there aren't timer connected to every pin.
Oh no!
</p>
<p>
Okay, that's not quite true, the timers are connected to every pin.
It is just the case that these connections take an unfortunate route: straight through the CPU via the timer interrupt.
This library just sets up a timer to periodically fire an interrupt and checks if some GPIO pins should be toggled.
The upside is that you can pretty much use any GPIO pin and are not bound by the whims of whoever decided on the pin alternate modes.
The downside is that you have to go through the CPU, while dedicated hardware could work independently.
And if the CPU is busy with some other interrupt, this one will have to wait and flip the pin too late, causing the 80µs delay we observed.
</p>
<p>
That is quite the footgun to put into an innocent sounding library for hobbyists that probably haven't taken a course in realtime systems.
</p>
<p>
But having an 80µs interrupt is quite a lot: at 16MHz that's 1280 cycles, you have to screw up a lot to use this many cycles.
Who is doing this screwing in an upwards direction?
You might have guessed it: the interrupts for reading the PWM signals.
This also explains why the twitching behavior seems to drift in and out: both the radio receiver as well as the Arduino try to produce a signal with the same frequency, but since they run on different clocks, slight timing differences make them shift in and out of phase.
</p>
<code id="listing1">
<pre>
<span>...</span>
<span>volatile int rcSteering;</span>
<span>volatile uint32_t steeringHighTime;</span>
<span>volatile uint32_t steeringLowTime;</span>
<span>volatile int rcDrive;</span>
<span>volatile uint32_t driveHighTime;</span>
<span>volatile uint32_t driveLowTime;</span>
<span>...</span>
<span>void setup() {</span>
<span> ...</span>
<span> pinMode(1, INPUT);</span>
<span> attachInterrupt(3, interupt_rcDrive, CHANGE); </span>
<span></span>
<span> pinMode(0, INPUT);</span>
<span> attachInterrupt(2, interupt_rcSteering, CHANGE);</span>
<span> ...</span>
<span>}</span>
<span>...</span>
<span>void interupt_rcDrive() {</span>
<span> if (digitalRead(1)) {</span>
<span> long currentTime = micros(); </span>
<span> rcDrive = (1000 * (driveLowTime - driveHighTime)) / (currentTime - driveHighTime);</span>
<span> driveHighTime = currentTime;</span>
<span> } else {</span>
<span> driveLowTime = micros();</span>
<span> }</span>
<span>}</span>
<span></span>
<span>void interupt_rcSteering() {</span>
<span> // pretty much the same as above</span>
<span>}</span>
<span>...</span>
<span>void update() {</span>
<span> ...</span>
<span> Serial.write(rcDrive);</span>
<span> Serial.write(rcSteering);</span>
<span> ...</span>
<span>}</span>
<span>...</span>
</pre>
<figcaption>Listing 1</figcaption>
</code>
<p>
It took me a while to comprehend what was happening in this interrupt (note that you get to see the cleaned up version).
The <code>else</code> branch is quite uninteresting, so let's focus on what happens when <code>digitalRead(1)</code> returns true.
The code is juggling three timestamps: <code>driveHighTime</code> when the signal last went high, <code>driveLowTime</code> when the signal goes low and finally <code>currentTime</code> is the current time where the signal went high.
With these timestamps two durations are calculated: <code>driveLowTime - driveHighTime</code> is how many µs the signal was high for and <code>currentTime - driveHighTime</code> is the length of the entire signal in µs.
The first duration is then divided by the second to get the duty-cycle, and then there's a multiplication by 1000 in there for good measure, since everything uses integers, not floats.
</p>
<p>
Okay, as mentioned earlier, RC equipment cares about timing, not duty-cycles, so if for some reason the period changes, e.g. because we changed the radio receiver, the code might not work anymore.
But the much much bigger problem is the division operation.
I'm sure you've already heard that "divisions are expensive".
And while the truth of that statement heavily depends on the circumstances, here it is almost an understatement.
This code is asking an 8-bit CPU that can barely do multiplications to do a division of two 32-bit variables.
Since both sides of the operation are variables, there is also no chance that the compiler could do any smart optimizations to make this less of a disaster.
<a href="https://godbolt.org/z/n8de1TG4Y" target="_blank">Here's a link</a> that shows how bad just one of these interrupts is.
Though it doesn't even show the full picture because the <a href="https://github.com/gcc-mirror/gcc/blob/master/libgcc/config/avr/lib1funcs.S#L621" target="_blank">multiplication</a> and <a href="https://github.com/gcc-mirror/gcc/blob/master/libgcc/config/avr/lib1funcs.S#L1633" target="_blank">division</a> functions is just called and not shown.
</p>
</section>
<section>
<h3>Could you just not...</h3>
<p>
... do 99% of this work in the ISR?
Why yes you could!
All we really care about is the timestamp, any calculation can be done later outside the ISR.
So all we need is this:
</p>
<code id="listing2">
<pre>
<span>...</span>
<span>uint8_t rcDrive;</span>
<span>volatile uint32_t driveHighTime;</span>
<span>volatile uint32_t driveLowTime;</span>
<span>...</span>
<span>void interupt_rcDrive() {</span>
<span> if (digitalRead(1)) {</span>
<span> driveHighTime = micros();</span>
<span> driveLowTime = 0;</span>
<span> } else {</span>
<span> driveLowTime = micros();</span>
<span> }</span>
<span>}</span>
<span>...</span>
</pre>
<figcaption>Listing 2</figcaption>
</code>
<p>
Here the low time is set to 0 on a rising edge, so the outside code knows that a new measurement is in progress and the numbers aren't ready yet.
This makes some assumptions about the other code that reads the timestamps, for example that it doesn't happen to always read <code>driveLowTime</code> when it is 0.
But in practice this worked perfectly so far.
Speaking of the reading code:
</p>
<code id="listing3">
<pre>
<span>...</span>
<span>uint8_t rcDrive;</span>
<span>volatile uint32_t driveHighTime;</span>
<span>volatile uint32_t driveLowTime;</span>
<span>...</span>
<span>void update() {</span>
<span> ...</span>
<span> // Temporarily disable interrupts while we read the variables they touch.</span>
<span> cli();</span>
<span> if (driveLowTime) {</span>
<span> // The high time is expected to be between 1000µs and 2000µs, for which a signed 16-bit</span>
<span> // variable has plenty of space.</span>
<span> int16_t high_us = int16_t(driveLowTime - driveHighTime);</span>
<span> sei();</span>
<span></span>
<span> // 1500µs is the neutral point, deviations up to +-512 are expected.</span>
<span> int16_t deviation_us = high_us - 1500;</span>
<span> if (deviation_us < -512) deviation_us = -512;</span>
<span> if ( 512 < deviation_us) deviation_us = 512;</span>
<span></span>
<span> // The output's neutral value is 90 with deviations up to +-8. 64 perfectly divides 512 into 8</span>
<span> // and also allows the compiler to use shifts instead of division.</span>
<span> rcDrive = uint8_t(90 + (deviation_us / 64));</span>
<span> } else {</span>
<span> sei();</span>
<span> }</span>
<span> Serial.write(rcDrive);</span>
<span> ...</span>
<span>}</span>
<span>...</span>
</pre>
<figcaption>Listing 3</figcaption>
</code>
<p>
I also took the liberty to do the calculation based on the deviation from 1.5ms, instead of calculating the duty-cycle.
This was not only for my sanity, but also necessary since the code no longer has three timestamps to work with.
<a href="https://godbolt.org/z/z1sWc33rP" target="_blank">Here's</a> the new code.
While the output is slightly longer, this time there are no calls to functions that do long multiplications or divisions.
And especially the interrupt is now only 17 instructions on the longer of the two paths.
While writing this article, I noticed that it could be even shorter by turning the timestamps into 16-bit variables.
So there are even more gains to be had.
</p>
</section>
<section>
<h3>Conclusion</h3>
<p>
And that's pretty much it.
After figuring out the convoluted build and flash process for the code, I tested these changes and the car has been much more quiet since.
So yeah: don't do heavy work inside interrupts.
</p>
<p>
There are still plenty of ways this could be improved: for example the PWM output is still half done in software.
But there are enough other things to fix elsewhere, maybe I'll find something else to write about.
</p>
</section>
<section>
<h2>Change-log</h2>
<ul>
<li>2022-08-15 - Fixed more spelling.</li>
<li>2022-06-15 - Fixed typos.</li>
<li>2022-06-13 - Improved wording, fixed typos.</li>
<li>2022-06-12 - Initial post.</li>
</ul>
</section>
</article>
<footer>
<a href="changelog.html">Change-log</a>
</footer>
</body>
</html>