Skip to content

Commit 2534efb

Browse files
ThomasFarstrikeThomasFarstrike
committed
webcam: add support for outputting rgb565 format
1 parent fbee73c commit 2534efb

File tree

2 files changed

+109
-43
lines changed

2 files changed

+109
-43
lines changed

‎c_mpos/src/webcam.c‎

Lines changed: 108 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,14 @@
77
#include <sys/mman.h>
88
#include <string.h>
99
#include <errno.h>
10+
#include <stdint.h>
1011
#include "py/obj.h"
1112
#include "py/runtime.h"
1213
#include "py/mperrno.h"
1314

1415
#define WIDTH 640
1516
#define HEIGHT 480
16-
#define NUM_BUFFERS 1 // more buffers doesnt seem to help so one is enough
17+
#define NUM_BUFFERS 1
1718
#define OUTPUT_WIDTH 240
1819
#define OUTPUT_HEIGHT 240
1920

@@ -27,26 +28,61 @@ typedef struct _webcam_obj_t {
2728
void *buffers[NUM_BUFFERS];
2829
size_t buffer_length;
2930
int frame_count;
30-
unsigned char *gray_buffer;
31+
unsigned char *gray_buffer; // For grayscale
32+
uint16_t *rgb565_buffer; // For RGB565
3133
} webcam_obj_t;
3234

33-
static void yuyv_to_grayscale_240x240(unsigned char *yuyv, unsigned char *gray, int in_width, int in_height) {
34-
// Crop to 480x480 centered region
35+
static void yuyv_to_rgb565_240x240(unsigned char *yuyv, uint16_t *rgb565, int in_width, int in_height) {
3536
int crop_size = 480;
36-
int crop_x_offset = (in_width - crop_size) / 2; // Center the crop: (640 - 480) / 2 = 80
37-
int crop_y_offset = (in_height - crop_size) / 2; // Center the crop: (480 - 480) / 2 = 0
37+
int crop_x_offset = (in_width - crop_size) / 2;
38+
int crop_y_offset = (in_height - crop_size) / 2;
39+
float x_ratio = (float)crop_size / OUTPUT_WIDTH;
40+
float y_ratio = (float)crop_size / OUTPUT_HEIGHT;
41+
42+
for (int y = 0; y < OUTPUT_HEIGHT; y++) {
43+
for (int x = 0; x < OUTPUT_WIDTH; x++) {
44+
int src_x = (int)(x * x_ratio) + crop_x_offset;
45+
int src_y = (int)(y * y_ratio) + crop_y_offset;
46+
int src_index = (src_y * in_width + src_x) * 2;
47+
48+
int y0 = yuyv[src_index];
49+
int u = yuyv[src_index + 1];
50+
int v = yuyv[src_index + 3];
51+
52+
int c = y0 - 16;
53+
int d = u - 128;
54+
int e = v - 128;
55+
56+
int r = (298 * c + 409 * e + 128) >> 8;
57+
int g = (298 * c - 100 * d - 208 * e + 128) >> 8;
58+
int b = (298 * c + 516 * d + 128) >> 8;
59+
60+
r = r < 0 ? 0 : (r > 255 ? 255 : r);
61+
g = g < 0 ? 0 : (g > 255 ? 255 : g);
62+
b = b < 0 ? 0 : (b > 255 ? 255 : b);
63+
64+
uint16_t r5 = (r >> 3) & 0x1F;
65+
uint16_t g6 = (g >> 2) & 0x3F;
66+
uint16_t b5 = (b >> 3) & 0x1F;
67+
68+
rgb565[y * OUTPUT_WIDTH + x] = (r5 << 11) | (g6 << 5) | b5;
69+
}
70+
}
71+
}
3872

39-
// Downscale ratios from 480x480 to 240x240
40-
float x_ratio = (float)crop_size / OUTPUT_WIDTH; // 480 / 240 = 2.0
41-
float y_ratio = (float)crop_size / OUTPUT_HEIGHT; // 480 / 240 = 2.0
73+
static void yuyv_to_grayscale_240x240(unsigned char *yuyv, unsigned char *gray, int in_width, int in_height) {
74+
int crop_size = 480;
75+
int crop_x_offset = (in_width - crop_size) / 2;
76+
int crop_y_offset = (in_height - crop_size) / 2;
77+
float x_ratio = (float)crop_size / OUTPUT_WIDTH;
78+
float y_ratio = (float)crop_size / OUTPUT_HEIGHT;
4279

4380
for (int y = 0; y < OUTPUT_HEIGHT; y++) {
4481
for (int x = 0; x < OUTPUT_WIDTH; x++) {
45-
// Map output pixel to cropped region
46-
int src_x = (int)(x * x_ratio) + crop_x_offset; // Adjust for crop offset
47-
int src_y = (int)(y * y_ratio) + crop_y_offset; // Adjust for crop offset
48-
int src_index = (src_y * in_width + src_x) * 2; // YUYV uses 2 bytes per pixel
49-
gray[y * OUTPUT_WIDTH + x] = yuyv[src_index]; // Extract Y channel
82+
int src_x = (int)(x * x_ratio) + crop_x_offset;
83+
int src_y = (int)(y * y_ratio) + crop_y_offset;
84+
int src_index = (src_y * in_width + src_x) * 2;
85+
gray[y * OUTPUT_WIDTH + x] = yuyv[src_index];
5086
}
5187
}
5288
}
@@ -61,6 +97,16 @@ static void save_raw(const char *filename, unsigned char *data, int width, int h
6197
fclose(fp);
6298
}
6399

100+
static void save_raw_rgb565(const char *filename, uint16_t *data, int width, int height) {
101+
FILE *fp = fopen(filename, "wb");
102+
if (!fp) {
103+
WEBCAM_DEBUG_PRINT("Cannot open file %s: %s\n", filename, strerror(errno));
104+
return;
105+
}
106+
fwrite(data, sizeof(uint16_t), width * height, fp);
107+
fclose(fp);
108+
}
109+
64110
static int init_webcam(webcam_obj_t *self, const char *device) {
65111
self->fd = open(device, O_RDWR);
66112
if (self->fd < 0) {
@@ -127,9 +173,12 @@ static int init_webcam(webcam_obj_t *self, const char *device) {
127173
}
128174

129175
self->frame_count = 0;
130-
self->gray_buffer = (unsigned char *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT);
131-
if (!self->gray_buffer) {
132-
WEBCAM_DEBUG_PRINT("Cannot allocate gray buffer: %s\n", strerror(errno));
176+
self->gray_buffer = (unsigned char *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT * sizeof(unsigned char));
177+
self->rgb565_buffer = (uint16_t *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT * sizeof(uint16_t));
178+
if (!self->gray_buffer || !self->rgb565_buffer) {
179+
WEBCAM_DEBUG_PRINT("Cannot allocate buffers: %s\n", strerror(errno));
180+
free(self->gray_buffer);
181+
free(self->rgb565_buffer);
133182
close(self->fd);
134183
return -1;
135184
}
@@ -148,24 +197,25 @@ static void deinit_webcam(webcam_obj_t *self) {
148197
}
149198
}
150199

151-
if (self->gray_buffer) {
152-
free(self->gray_buffer);
153-
self->gray_buffer = NULL;
154-
}
200+
free(self->gray_buffer);
201+
self->gray_buffer = NULL;
202+
free(self->rgb565_buffer);
203+
self->rgb565_buffer = NULL;
155204

156205
close(self->fd);
157206
self->fd = -1;
158207
}
159208

160209
static mp_obj_t free_buffer(webcam_obj_t *self) {
161-
if (self->gray_buffer) {
162-
free(self->gray_buffer);
163-
self->gray_buffer = NULL;
164-
}
210+
free(self->gray_buffer);
211+
self->gray_buffer = NULL;
212+
free(self->rgb565_buffer);
213+
self->rgb565_buffer = NULL;
165214
return mp_const_none;
166215
}
167216

168-
static mp_obj_t capture_frame(webcam_obj_t *self) {
217+
static mp_obj_t capture_frame(mp_obj_t self_in, mp_obj_t format) {
218+
webcam_obj_t *self = MP_OBJ_TO_PTR(self_in);
169219
struct v4l2_buffer buf = {0};
170220
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
171221
buf.memory = V4L2_MEMORY_MMAP;
@@ -174,25 +224,40 @@ static mp_obj_t capture_frame(webcam_obj_t *self) {
174224
}
175225

176226
if (!self->gray_buffer) {
177-
self->gray_buffer = (unsigned char *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT);
227+
self->gray_buffer = (unsigned char *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT * sizeof(unsigned char));
178228
if (!self->gray_buffer) {
179229
mp_raise_OSError(MP_ENOMEM);
180230
}
181231
}
182-
183-
yuyv_to_grayscale_240x240(self->buffers[buf.index], self->gray_buffer, WIDTH, HEIGHT);
184-
185-
// char filename[32];
186-
// snprintf(filename, sizeof(filename), "frame_%03d.raw", self->frame_count++);
187-
// save_raw(filename, self->gray_buffer, OUTPUT_WIDTH, OUTPUT_HEIGHT);
188-
189-
mp_obj_t result = mp_obj_new_memoryview('b', OUTPUT_WIDTH * OUTPUT_HEIGHT, self->gray_buffer);
190-
191-
if (ioctl(self->fd, VIDIOC_QBUF, &buf) < 0) {
192-
mp_raise_OSError(MP_EIO);
232+
if (!self->rgb565_buffer) {
233+
self->rgb565_buffer = (uint16_t *)malloc(OUTPUT_WIDTH * OUTPUT_HEIGHT * sizeof(uint16_t));
234+
if (!self->rgb565_buffer) {
235+
mp_raise_OSError(MP_ENOMEM);
236+
}
193237
}
194238

195-
return result;
239+
const char *fmt = mp_obj_str_get_str(format);
240+
if (strcmp(fmt, "grayscale") == 0) {
241+
yuyv_to_grayscale_240x240(self->buffers[buf.index], self->gray_buffer, WIDTH, HEIGHT);
242+
// char filename[32];
243+
// snprintf(filename, sizeof(filename), "frame_%03d.raw", self->frame_count++);
244+
// save_raw(filename, self->gray_buffer, OUTPUT_WIDTH, OUTPUT_HEIGHT);
245+
mp_obj_t result = mp_obj_new_memoryview('B', OUTPUT_WIDTH * OUTPUT_HEIGHT, self->gray_buffer);
246+
if (ioctl(self->fd, VIDIOC_QBUF, &buf) < 0) {
247+
mp_raise_OSError(MP_EIO);
248+
}
249+
return result;
250+
} else {
251+
yuyv_to_rgb565_240x240(self->buffers[buf.index], self->rgb565_buffer, WIDTH, HEIGHT);
252+
// char filename[32];
253+
// snprintf(filename, sizeof(filename), "frame_%03d.rgb565", self->frame_count++);
254+
// save_raw_rgb565(filename, self->rgb565_buffer, OUTPUT_WIDTH, OUTPUT_HEIGHT);
255+
mp_obj_t result = mp_obj_new_memoryview('H', OUTPUT_WIDTH * OUTPUT_HEIGHT, self->rgb565_buffer);
256+
if (ioctl(self->fd, VIDIOC_QBUF, &buf) < 0) {
257+
mp_raise_OSError(MP_EIO);
258+
}
259+
return result;
260+
}
196261
}
197262

198263
static mp_obj_t webcam_init(size_t n_args, const mp_obj_t *args) {
@@ -206,6 +271,7 @@ static mp_obj_t webcam_init(size_t n_args, const mp_obj_t *args) {
206271
self->base.type = &webcam_type;
207272
self->fd = -1;
208273
self->gray_buffer = NULL;
274+
self->rgb565_buffer = NULL;
209275

210276
if (init_webcam(self, device) < 0) {
211277
mp_raise_OSError(MP_EIO);
@@ -228,14 +294,14 @@ static mp_obj_t webcam_free_buffer(mp_obj_t self_in) {
228294
}
229295
MP_DEFINE_CONST_FUN_OBJ_1(webcam_free_buffer_obj, webcam_free_buffer);
230296

231-
static mp_obj_t webcam_capture_frame(mp_obj_t self_in) {
297+
static mp_obj_t webcam_capture_frame(mp_obj_t self_in, mp_obj_t format) {
232298
webcam_obj_t *self = MP_OBJ_TO_PTR(self_in);
233299
if (self->fd < 0) {
234300
mp_raise_OSError(MP_EIO);
235301
}
236-
return capture_frame(self);
302+
return capture_frame(self, format);
237303
}
238-
MP_DEFINE_CONST_FUN_OBJ_1(webcam_capture_frame_obj, webcam_capture_frame);
304+
MP_DEFINE_CONST_FUN_OBJ_2(webcam_capture_frame_obj, webcam_capture_frame);
239305

240306
static const mp_obj_type_t webcam_type = {
241307
{ &mp_type_type },

‎internal_filesystem/apps/com.example.camtest/assets/camtest.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ def qr_button_click(e):
9999
def try_capture():
100100
global current_cam_buffer, image_dsc, image, use_webcam
101101
if use_webcam:
102-
current_cam_buffer = webcam.capture_frame(cam)
102+
current_cam_buffer = webcam.capture_frame(cam, "rgb565")
103103
elif cam.frame_available():
104104
current_cam_buffer = cam.capture() # Returns memoryview
105105
if current_cam_buffer and len(current_cam_buffer):

0 commit comments

Comments
 (0)