mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Merge pull request #2793 from openmv/fix_lepton_sync
common/vospi: Fix synchronization issues.
This commit is contained in:
commit
62a652044f
133
common/vospi.c
133
common/vospi.c
@ -37,36 +37,47 @@
|
|||||||
#include "omv_common.h"
|
#include "omv_common.h"
|
||||||
#include "omv_spi.h"
|
#include "omv_spi.h"
|
||||||
|
|
||||||
#define VOSPI_HEADER_WORDS (2) // 16-bits
|
#define VOSPI_HEADER_WORDS (2) // 16-bits
|
||||||
#define VOSPI_PID_SIZE_PIXELS (80) // w, 16-bits per pixel
|
#define VOSPI_PID_SIZE_PIXELS (80) // w, 16-bits per pixel
|
||||||
#define VOSPI_PIDS_PER_SID (60) // h
|
#define VOSPI_PIDS_PER_SID (60) // h
|
||||||
#define VOSPI_SIDS_PER_FRAME (4)
|
#define VOSPI_SIDS_PER_FRAME(vospi) (VOSPI_IS_LEPTON3(vospi) ? 4 : 1)
|
||||||
#define VOSPI_PACKET_SIZE (VOSPI_HEADER_WORDS + VOSPI_PID_SIZE_PIXELS) // 16-bits
|
#define VOSPI_PACKET_SIZE (VOSPI_HEADER_WORDS + VOSPI_PID_SIZE_PIXELS) // 16-bits
|
||||||
#define VOSPI_SID_SIZE_PIXELS (VOSPI_PIDS_PER_SID * VOSPI_PID_SIZE_PIXELS) // 16-bits
|
#define VOSPI_SID_SIZE_PIXELS (VOSPI_PIDS_PER_SID * VOSPI_PID_SIZE_PIXELS) // 16-bits
|
||||||
|
|
||||||
#define VOSPI_BUFFER_SIZE (VOSPI_PACKET_SIZE * 2) // 16-bits
|
#define VOSPI_BUFFER_SIZE (VOSPI_PACKET_SIZE * 2) // 16-bits
|
||||||
#define VOSPI_CLOCK_SPEED 20000000 // hz
|
#define VOSPI_CLOCK_SPEED 20000000 // hz
|
||||||
#define VOSPI_SYNC_MS 200 // ms
|
#define VOSPI_SYNC_DELAY_MS 250 // ms
|
||||||
|
#define VOSPI_SYNC_TIMEOUT_MS 100
|
||||||
|
|
||||||
#define VOSPI_DONT_CARE_PACKET (0x0F00)
|
// Packet Header Format (32 bits total: ID Field + CRC)
|
||||||
#define VOSPI_HEADER_DONT_CARE(x) (((x) & VOSPI_DONT_CARE_PACKET) == VOSPI_DONT_CARE_PACKET)
|
// ┌───┬───────────┬──────────────────┬───────────────┐
|
||||||
#define VOSPI_HEADER_PID(id) ((id) & 0x0FFF)
|
// │ 0 │ 3-bit SID │ 12-bit PID (0–20)│ 16-bit CRC │
|
||||||
#define VOSPI_HEADER_SID(id) (((id) >> 12) & 0x7)
|
// └───┴───────────┴──────────────────┴───────────────┘
|
||||||
#define VOSPI_IS_SPECIAL_PACKET(pid) (vospi.lepton_3 && pid == 20)
|
// Notes:
|
||||||
|
// - Packet numbers restart at 0 on each new segment.
|
||||||
|
// - Discard packets with PID 0x0Fxx.
|
||||||
|
// - On packet 20, the SID bits encode the segment number (1–4).
|
||||||
|
#define VOSPI_HEADER_PID(id) ((id) & 0x0FFFu)
|
||||||
|
#define VOSPI_HEADER_SID(id) (((id) >> 12u) & 0x7u)
|
||||||
|
#define VOSPI_IS_LEPTON3(vospi) (vospi.n_packets > VOSPI_PIDS_PER_SID)
|
||||||
|
#define VOSPI_IS_PID_VALID(vospi, pid) (((pid) & 0x0F00u) != 0x0F00u)
|
||||||
|
#define VOSPI_IS_SID_VALID(vospi, pid) (VOSPI_IS_LEPTON3(vospi) && pid == 20)
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
VOSPI_FLAG_STREAM = (1 << 0),
|
VOSPI_FLAG_STREAM = (1 << 0),
|
||||||
VOSPI_FLAG_CAPTURE = (1 << 1),
|
VOSPI_FLAG_CAPTURE = (1 << 1),
|
||||||
|
VOSPI_FLAG_SYNC_ERROR = (1 << 2),
|
||||||
} vospi_flags_t;
|
} vospi_flags_t;
|
||||||
|
|
||||||
typedef struct _vospi_state {
|
typedef struct _vospi_state {
|
||||||
int pid;
|
size_t pid;
|
||||||
int sid;
|
size_t sid;
|
||||||
framebuffer_t *fb;
|
framebuffer_t *fb;
|
||||||
bool lepton_3;
|
|
||||||
omv_spi_t spi_bus;
|
omv_spi_t spi_bus;
|
||||||
|
size_t n_packets;
|
||||||
|
uint32_t last_abort_ms;
|
||||||
|
size_t last_sync_ms;
|
||||||
volatile uint32_t flags;
|
volatile uint32_t flags;
|
||||||
uint32_t resync_ms;
|
|
||||||
} vospi_state_t;
|
} vospi_state_t;
|
||||||
|
|
||||||
static vospi_state_t vospi;
|
static vospi_state_t vospi;
|
||||||
@ -94,22 +105,28 @@ static bool vospi_check_crc(const uint16_t *base) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
static void vospi_callback(omv_spi_t *spi, void *userdata, void *buf) {
|
static void vospi_callback(omv_spi_t *spi, void *userdata, void *buf) {
|
||||||
|
vbuffer_t *buffer = NULL;
|
||||||
|
const uint16_t *base = (uint16_t *) buf;
|
||||||
|
|
||||||
if (!(vospi.flags & VOSPI_FLAG_CAPTURE) ||
|
if (!(vospi.flags & VOSPI_FLAG_CAPTURE) ||
|
||||||
!(vospi.flags & VOSPI_FLAG_STREAM)) {
|
!(vospi.flags & VOSPI_FLAG_STREAM)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
const uint16_t *base = (uint16_t *) buf;
|
size_t pid = VOSPI_HEADER_PID(base[0]);
|
||||||
|
size_t sid = VOSPI_HEADER_SID(base[0]) - 1; // SID is one-based
|
||||||
|
|
||||||
int id = base[0];
|
// Discard invalid/don't care packets.
|
||||||
|
if (!VOSPI_IS_PID_VALID(vospi, pid)) {
|
||||||
// Ignore don't care packets.
|
|
||||||
if (VOSPI_HEADER_DONT_CARE(id)) {
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
int pid = VOSPI_HEADER_PID(id);
|
// Abort the VoSPI stream if not sync'ing for too long.
|
||||||
int sid = VOSPI_HEADER_SID(id) - 1;
|
if ((mp_hal_ticks_ms() - vospi.last_sync_ms) >= VOSPI_SYNC_TIMEOUT_MS) {
|
||||||
|
vospi.flags |= VOSPI_FLAG_SYNC_ERROR;
|
||||||
|
vospi_abort();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// Discard packets with a pid != 0 when waiting for the first packet.
|
// Discard packets with a pid != 0 when waiting for the first packet.
|
||||||
if (vospi.pid == 0 && pid != 0) {
|
if (vospi.pid == 0 && pid != 0) {
|
||||||
@ -117,22 +134,22 @@ static void vospi_callback(omv_spi_t *spi, void *userdata, void *buf) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Discard segments with a sid != 0 when waiting for the first segment.
|
// Discard segments with a sid != 0 when waiting for the first segment.
|
||||||
if (VOSPI_IS_SPECIAL_PACKET(pid) && vospi.sid == 0 && sid != 0) {
|
if (vospi.sid == 0 && sid != 0 && VOSPI_IS_SID_VALID(vospi, pid)) {
|
||||||
vospi.pid = 0;
|
vospi.pid = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Are we in sync with the flir lepton?
|
// Check if packet is in sync with the expected VoSPI stream
|
||||||
if (pid != vospi.pid ||
|
if (pid != vospi.pid || !vospi_check_crc(base) ||
|
||||||
!vospi_check_crc(base) ||
|
(VOSPI_IS_SID_VALID(vospi, pid) && sid != vospi.sid)) {
|
||||||
(VOSPI_IS_SPECIAL_PACKET(pid) && sid != vospi.sid)) {
|
|
||||||
vospi_abort();
|
vospi_abort();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
vbuffer_t *buffer = framebuffer_acquire(vospi.fb, FB_FLAG_FREE | FB_FLAG_PEEK);
|
// Update sync timestamp.
|
||||||
|
vospi.last_sync_ms = mp_hal_ticks_ms();
|
||||||
|
|
||||||
if (!buffer) {
|
if (!(buffer = framebuffer_acquire(vospi.fb, FB_FLAG_FREE | FB_FLAG_PEEK))) {
|
||||||
vospi.flags &= ~VOSPI_FLAG_CAPTURE;
|
vospi.flags &= ~VOSPI_FLAG_CAPTURE;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -144,26 +161,17 @@ static void vospi_callback(omv_spi_t *spi, void *userdata, void *buf) {
|
|||||||
|
|
||||||
if (++vospi.pid == VOSPI_PIDS_PER_SID) {
|
if (++vospi.pid == VOSPI_PIDS_PER_SID) {
|
||||||
vospi.pid = 0;
|
vospi.pid = 0;
|
||||||
|
if (++vospi.sid == VOSPI_SIDS_PER_FRAME(vospi)) {
|
||||||
// For the FLIR Lepton 3 we have to receive all the pids in all the segments.
|
vospi.sid = 0;
|
||||||
if (vospi.lepton_3) {
|
|
||||||
if (++vospi.sid == VOSPI_SIDS_PER_FRAME) {
|
|
||||||
vospi.sid = 0;
|
|
||||||
framebuffer_release(vospi.fb, FB_FLAG_FREE);
|
|
||||||
}
|
|
||||||
// For the FLIR Lepton 1/2 we just have to receive all the pids.
|
|
||||||
} else {
|
|
||||||
// Move the buffer from free queue -> used queue.
|
|
||||||
framebuffer_release(vospi.fb, FB_FLAG_FREE);
|
framebuffer_release(vospi.fb, FB_FLAG_FREE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int vospi_init(uint32_t n_packets, framebuffer_t *fb) {
|
int vospi_init(size_t n_packets, framebuffer_t *fb) {
|
||||||
memset(&vospi, 0, sizeof(vospi_state_t));
|
memset(&vospi, 0, sizeof(vospi_state_t));
|
||||||
vospi.fb = fb;
|
vospi.fb = fb;
|
||||||
vospi.lepton_3 = n_packets > VOSPI_PIDS_PER_SID;
|
vospi.n_packets = n_packets;
|
||||||
|
|
||||||
omv_spi_config_t spi_config;
|
omv_spi_config_t spi_config;
|
||||||
omv_spi_default_config(&spi_config, OMV_CSI_SPI_ID);
|
omv_spi_default_config(&spi_config, OMV_CSI_SPI_ID);
|
||||||
@ -189,29 +197,34 @@ bool vospi_active(void) {
|
|||||||
|
|
||||||
int vospi_abort(void) {
|
int vospi_abort(void) {
|
||||||
vospi.flags = 0;
|
vospi.flags = 0;
|
||||||
int ret = omv_spi_transfer_abort(&vospi.spi_bus);
|
omv_spi_deinit(&vospi.spi_bus);
|
||||||
vospi.pid = 0;
|
vospi.last_abort_ms = mp_hal_ticks_ms();
|
||||||
vospi.sid = 0;
|
return 0;
|
||||||
vospi.resync_ms = mp_hal_ticks_ms();
|
|
||||||
return ret;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void vospi_restart(void) {
|
void vospi_restart(void) {
|
||||||
omv_spi_transfer_t spi_xfer = {
|
// Update sync timestamp.
|
||||||
.rxbuf = vospi_buf,
|
vospi.last_sync_ms = mp_hal_ticks_ms();
|
||||||
.size = VOSPI_BUFFER_SIZE,
|
|
||||||
.flags = OMV_SPI_XFER_DMA,
|
|
||||||
.callback = vospi_callback,
|
|
||||||
};
|
|
||||||
|
|
||||||
// Resume streaming.
|
// Resume streaming.
|
||||||
vospi.flags |= VOSPI_FLAG_CAPTURE;
|
vospi.flags |= VOSPI_FLAG_CAPTURE;
|
||||||
|
vospi.flags &= ~VOSPI_FLAG_SYNC_ERROR;
|
||||||
|
|
||||||
if (!(vospi.flags & VOSPI_FLAG_STREAM) &&
|
if (!(vospi.flags & VOSPI_FLAG_STREAM) &&
|
||||||
(mp_hal_ticks_ms() - vospi.resync_ms) >= VOSPI_SYNC_MS) {
|
(mp_hal_ticks_ms() - vospi.last_abort_ms) >= VOSPI_SYNC_DELAY_MS) {
|
||||||
omv_spi_transfer_start(&vospi.spi_bus, &spi_xfer);
|
omv_spi_transfer_t spi_xfer = {
|
||||||
vospi.flags |= VOSPI_FLAG_STREAM;
|
.rxbuf = vospi_buf,
|
||||||
}
|
.size = VOSPI_BUFFER_SIZE,
|
||||||
|
.flags = OMV_SPI_XFER_DMA,
|
||||||
|
.callback = vospi_callback,
|
||||||
|
};
|
||||||
|
|
||||||
|
vospi_init(vospi.n_packets, vospi.fb);
|
||||||
|
|
||||||
|
// Restart VoSPI transfer.
|
||||||
|
vospi.flags |= VOSPI_FLAG_STREAM;
|
||||||
|
vospi.last_sync_ms = mp_hal_ticks_ms();
|
||||||
|
omv_spi_transfer_start(&vospi.spi_bus, &spi_xfer);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -26,8 +26,8 @@
|
|||||||
#ifndef __VOSPI_H__
|
#ifndef __VOSPI_H__
|
||||||
#define __VOSPI_H__
|
#define __VOSPI_H__
|
||||||
#include "framebuffer.h"
|
#include "framebuffer.h"
|
||||||
int vospi_init(uint32_t n_packets, framebuffer_t *fb);
|
int vospi_init(size_t n_packets, framebuffer_t *fb);
|
||||||
int vospi_deinit();
|
int vospi_deinit(void);
|
||||||
int vospi_abort(void);
|
int vospi_abort(void);
|
||||||
bool vospi_active(void);
|
bool vospi_active(void);
|
||||||
void vospi_restart(void);
|
void vospi_restart(void);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user