forked from google/periph
-
Notifications
You must be signed in to change notification settings - Fork 0
/
lepton.go
450 lines (429 loc) · 14.3 KB
/
lepton.go
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
// Copyright 2017 The Periph Authors. All rights reserved.
// Use of this source code is governed under the Apache License, Version 2.0
// that can be found in the LICENSE file.
package lepton
import (
"bytes"
"encoding/binary"
"errors"
"fmt"
"image"
"sync"
"time"
"periph.io/x/periph/conn"
"periph.io/x/periph/conn/i2c"
"periph.io/x/periph/conn/physic"
"periph.io/x/periph/conn/spi"
"periph.io/x/periph/devices/lepton/cci"
"periph.io/x/periph/devices/lepton/image14bit"
"periph.io/x/periph/devices/lepton/internal"
)
// Metadata is constructed from telemetry data, which is sent with each frame.
type Metadata struct {
SinceStartup time.Duration //
FrameCount uint32 // Number of frames since the start of the camera, in 27fps (not 9fps).
AvgValue uint16 // Average value of the buffer.
Temp physic.Temperature // Temperature inside the camera.
TempHousing physic.Temperature // Camera housing temperature.
RawTemp uint16 //
RawTempHousing uint16 //
FFCSince time.Duration // Time since last internal calibration.
FFCTemp physic.Temperature // Temperature at last internal calibration.
FFCTempHousing physic.Temperature //
FFCState cci.FFCState // Current calibration state.
FFCDesired bool // Asserted at start-up, after period (default 3m) or after temperature change (default 3K). Indicates that a calibration should be triggered as soon as possible.
Overtemp bool // true 10s before self-shutdown.
}
// Frame is a FLIR Lepton frame, containing 14 bits resolution intensity stored
// as image14bit.Gray14.
//
// Values centered around 8192 accorging to camera body temperature. Effective
// range is 14 bits, so [0, 16383].
//
// Each 1 increment is approximatively 0.025K.
type Frame struct {
*image14bit.Gray14
Metadata Metadata // Metadata that is sent along the pixels.
}
// New returns an initialized connection to the FLIR Lepton.
//
// Maximum SPI speed is 20Mhz. Minimum usable rate is ~2.2Mhz to sustain a 9hz
// framerate at 80x60.
//
// Maximum I²C speed is 1Mhz.
//
// MOSI is not used and should be grounded.
func New(p spi.Port, i i2c.Bus) (*Dev, error) {
// TODO(maruel): Switch to 16 bits per word, so that big endian 16 bits word
// decoding is done by the SPI driver.
s, err := p.Connect(20*physic.MegaHertz, spi.Mode3, 8)
if err != nil {
return nil, err
}
c, err := cci.New(i)
if err != nil {
return nil, err
}
// TODO(maruel): Support Lepton 3 with 160x120.
w := 80
h := 60
// telemetry data is a 3 lines header.
frameLines := h + 3
frameWidth := w*2 + 4
d := &Dev{
Dev: c,
s: s,
w: w,
h: h,
prevImg: image14bit.NewGray14(image.Rect(0, 0, w, h)),
frameWidth: frameWidth,
frameLines: frameLines,
delay: time.Second,
}
if l, ok := s.(conn.Limits); ok {
d.maxTxSize = l.MaxTxSize()
}
if status, err := d.GetStatus(); err != nil {
return nil, err
} else if status.CameraStatus != cci.SystemReady {
// The lepton takes < 1 second to boot so it should not happen normally.
return nil, fmt.Errorf("lepton: camera is not ready: %#v", status)
}
if err := d.Init(); err != nil {
return nil, err
}
return d, nil
}
// Dev controls a FLIR Lepton.
//
// It assumes a specific breakout board. Sadly the breakout board doesn't
// expose the PWR_DWN_L and RESET_L lines so it is impossible to shut down the
// Lepton.
type Dev struct {
*cci.Dev
s spi.Conn
w int
h int
prevImg *image14bit.Gray14
frameA, frameB []byte
frameWidth int // in bytes
frameLines int
maxTxSize int
delay time.Duration
}
func (d *Dev) String() string {
return fmt.Sprintf("Lepton(%s/%s)", d.Dev, d.s)
}
// Halt implements conn.Resource.
func (d *Dev) Halt() error {
// TODO(maruel): Stop the read loop.
return d.Dev.Halt()
}
// Bounds returns the device frame size.
func (d *Dev) Bounds() image.Rectangle {
return image.Rect(0, 0, d.w, d.h)
}
// NextFrame blocks and returns the next frame from the camera.
//
// It is ok to call other functions concurrently to send commands to the
// camera.
func (d *Dev) NextFrame(f *Frame) error {
if f.Bounds() != d.Bounds() {
return errors.New("lepton: invalid frame size")
}
for {
if err := d.readFrame(f); err != nil {
return err
}
if f.Metadata.FFCDesired {
// TODO(maruel): Automatically trigger FFC when applicable, only do if
// the camera has a shutter.
//go d.RunFFC()
}
// Sadly the Lepton will unconditionally send 27fps, even if the effective
// rate is 9fps.
if !equalUint16(d.prevImg.Pix, f.Gray14.Pix) {
break
}
// It also happen if the image is 100% static without noise.
}
copy(d.prevImg.Pix, f.Pix)
return nil
}
// Private details.
// stream reads continuously from the SPI connection.
func (d *Dev) stream(done <-chan struct{}, c chan<- []byte) error {
lines := 8
if d.maxTxSize != 0 {
if l := d.maxTxSize / d.frameWidth; l < lines {
lines = l
}
}
for {
// TODO(maruel): Use a ring buffer to stop continuously allocating.
buf := make([]byte, d.frameWidth*lines)
if err := d.s.Tx(nil, buf); err != nil {
return err
}
for i := 0; i < len(buf); i += d.frameWidth {
select {
case <-done:
return nil
case c <- buf[i : i+d.frameWidth]:
}
}
}
}
// readFrame reads one frame.
//
// Each frame is sent as a packet over SPI including telemetry data as an
// header. See page 49-57 for "VoSPI" protocol explanation.
//
// This operation must complete within 32ms. Frames occur every 38.4ms at
// almost 27hz.
//
// Resynchronization is done by deasserting CS and CLK for at least 5 frames
// (>185ms).
//
// When a packet starts, it must be completely clocked out within 3 line
// periods.
//
// One frame of 80x60 at 2 byte per pixel, plus 4 bytes overhead per line plus
// 3 lines of telemetry is (3+60)*(4+160) = 10332. The sysfs-spi driver limits
// each transaction size, the default is 4Kb. To reduce the risks of failure,
// reads 4Kb at a time and figure out the lines from there. The Lepton is very
// cranky if reading is not done quickly enough.
func (d *Dev) readFrame(f *Frame) error {
done := make(chan struct{}, 1)
c := make(chan []byte, 1024)
var err error
var wg sync.WaitGroup
wg.Add(1)
go func() {
defer wg.Done()
defer close(c)
err = d.stream(done, c)
}()
defer func() {
done <- struct{}{}
}()
timeout := time.After(d.delay)
w := f.Bounds().Dx()
sync := 0
discard := 0
for {
select {
case <-timeout:
return fmt.Errorf("failed to synchronize after %s", d.delay)
case l, ok := <-c:
if !ok {
wg.Wait()
return err
}
h := internal.Big16.Uint16(l)
if h&packetHeaderDiscard == packetHeaderDiscard {
discard++
sync = 0
continue
}
headerID := h & packetHeaderMask
if discard != 0 {
//log.Printf("discarded %d", discard)
discard = 0
sync = 0
}
if int(headerID) == 0 && sync == 0 && !verifyCRC(l) {
//log.Printf("no crc")
sync = 0
continue
}
if int(headerID) != sync {
//log.Printf("%d != %d", headerID, sync)
sync = 0
continue
}
if sync == 0 {
// Parse the first row of telemetry data.
if err2 := f.Metadata.parseTelemetry(l[4:]); err2 != nil {
//log.Printf("Failed to parse telemetry line: %v", err2)
continue
}
} else if sync >= 3 {
// Image.
for x := 0; x < w; x++ {
o := 4 + x*2
f.SetIntensity14(x, sync-3, image14bit.Intensity14(internal.Big16.Uint16(l[o:o+2])))
}
}
if sync++; sync == d.frameLines {
// Last line, done.
return nil
}
}
}
}
func (m *Metadata) parseTelemetry(data []byte) error {
// Telemetry line.
var rowA telemetryRowA
if err := binary.Read(bytes.NewBuffer(data), internal.Big16, &rowA); err != nil {
return err
}
m.SinceStartup = rowA.TimeCounter.Duration()
m.FrameCount = rowA.FrameCounter
m.AvgValue = rowA.FrameMean
m.Temp = rowA.FPATemp.Temperature()
m.TempHousing = rowA.HousingTemp.Temperature()
m.RawTemp = rowA.FPATempCounts
m.RawTempHousing = rowA.HousingTempCounts
m.FFCSince = rowA.TimeCounterLastFFC.Duration()
m.FFCTemp = rowA.FPATempLastFFC.Temperature()
m.FFCTempHousing = rowA.HousingTempLastFFC.Temperature()
if rowA.StatusBits&statusMaskNil != 0 {
return fmt.Errorf("lepton: (Status: 0x%08X) & (Mask: 0x%08X) = (Extra: 0x%08X) in 0x%08X", rowA.StatusBits, statusMask, rowA.StatusBits&statusMaskNil, statusMaskNil)
}
m.FFCDesired = rowA.StatusBits&statusFFCDesired != 0
m.Overtemp = rowA.StatusBits&statusOvertemp != 0
fccstate := rowA.StatusBits & statusFFCStateMask >> statusFFCStateShift
if rowA.TelemetryRevision == 8 {
switch fccstate {
case 0:
m.FFCState = cci.FFCNever
case 1:
m.FFCState = cci.FFCInProgress
case 2:
m.FFCState = cci.FFCComplete
default:
return fmt.Errorf("unexpected fccstate %d; %v", fccstate, data)
}
} else {
switch fccstate {
case 0:
m.FFCState = cci.FFCNever
case 2:
m.FFCState = cci.FFCInProgress
case 3:
m.FFCState = cci.FFCComplete
default:
return fmt.Errorf("unexpected fccstate %d; %v", fccstate, data)
}
}
return nil
}
// As documented as page.21
const (
packetHeaderDiscard = 0x0F00
packetHeaderMask = 0x0FFF // ID field is 12 bits. Leading 4 bits are reserved.
// Observed status:
// 0x00000808
// 0x00007A01
// 0x00022200
// 0x01AD0000
// 0x02BF0000
// 0x1FFF0000
// 0x3FFF0001
// 0xDCD0FFFF
// 0xFFDCFFFF
statusFFCDesired uint32 = 1 << 3 // 0x00000008
statusFFCStateMask uint32 = 3 << 4 // 0x00000030
statusFFCStateShift uint32 = 4 //
statusReserved uint32 = 1 << 11 // 0x00000800
statusAGCState uint32 = 1 << 12 // 0x00001000
statusOvertemp uint32 = 1 << 20 // 0x00100000
statusMask = statusFFCDesired | statusFFCStateMask | statusAGCState | statusOvertemp | statusReserved // 0x00101838
statusMaskNil = ^statusMask // 0xFFEFE7C7
)
// telemetryRowA is the data structure returned after the frame as documented
// at p.19-20.
//
// '*' means the value observed in practice make sense.
// Value after '-' is observed value.
type telemetryRowA struct {
TelemetryRevision uint16 // 0 *
TimeCounter internal.DurationMS // 1 *
StatusBits uint32 // 3 * Bit field (mostly make sense)
ModuleSerial [16]uint8 // 5 - Is empty (!)
SoftwareRevision uint64 // 13 Junk.
Reserved17 uint16 // 17 - 1101
Reserved18 uint16 // 18
Reserved19 uint16 // 19
FrameCounter uint32 // 20 *
FrameMean uint16 // 22 * The average value from the whole frame.
FPATempCounts uint16 // 23
FPATemp internal.CentiK // 24 *
HousingTempCounts uint16 // 25
HousingTemp internal.CentiK // 27 *
Reserved27 uint16 // 27
Reserved28 uint16 // 28
FPATempLastFFC internal.CentiK // 29 *
TimeCounterLastFFC internal.DurationMS // 30 *
HousingTempLastFFC internal.CentiK // 32 *
Reserved33 uint16 // 33
AGCROILeft uint16 // 35 * - 0 (Likely inversed, haven't confirmed)
AGCROITop uint16 // 34 * - 0
AGCROIRight uint16 // 36 * - 79 - SDK was wrong!
AGCROIBottom uint16 // 37 * - 59 - SDK was wrong!
AGCClipLimitHigh uint16 // 38 *
AGCClipLimitLow uint16 // 39 *
Reserved40 uint16 // 40 - 1
Reserved41 uint16 // 41 - 128
Reserved42 uint16 // 42 - 64
Reserved43 uint16 // 43
Reserved44 uint16 // 44
Reserved45 uint16 // 45
Reserved46 uint16 // 46
Reserved47 uint16 // 47 - 1
Reserved48 uint16 // 48 - 128
Reserved49 uint16 // 49 - 1
Reserved50 uint16 // 50
Reserved51 uint16 // 51
Reserved52 uint16 // 52
Reserved53 uint16 // 53
Reserved54 uint16 // 54
Reserved55 uint16 // 55
Reserved56 uint16 // 56 - 30
Reserved57 uint16 // 57
Reserved58 uint16 // 58 - 1
Reserved59 uint16 // 59 - 1
Reserved60 uint16 // 60 - 78
Reserved61 uint16 // 61 - 58
Reserved62 uint16 // 62 - 7
Reserved63 uint16 // 63 - 90
Reserved64 uint16 // 64 - 40
Reserved65 uint16 // 65 - 210
Reserved66 uint16 // 66 - 255
Reserved67 uint16 // 67 - 255
Reserved68 uint16 // 68 - 23
Reserved69 uint16 // 69 - 6
Reserved70 uint16 // 70
Reserved71 uint16 // 71
Reserved72 uint16 // 72 - 7
Reserved73 uint16 // 73
Log2FFCFrames uint16 // 74 Found 3, should be 27?
Reserved75 uint16 // 75
Reserved76 uint16 // 76
Reserved77 uint16 // 77
Reserved78 uint16 // 78
Reserved79 uint16 // 79
}
// verifyCRC test the equation x^16 + x^12 + x^5 + x^0
func verifyCRC(d []byte) bool {
tmp := make([]byte, len(d))
copy(tmp, d)
tmp[0] &^= 0x0F
tmp[2] = 0
tmp[3] = 0
return internal.CRC16(tmp) == internal.Big16.Uint16(d[2:])
}
func equalUint16(a, b []uint16) bool {
if len(a) != len(b) {
return false
}
for i := range a {
if a[i] != b[i] {
return false
}
}
return true
}
var _ conn.Resource = &Dev{}
var _ fmt.Stringer = &Dev{}