585 lines
16 KiB
C
585 lines
16 KiB
C
#include "io_pipeline.h"
|
|
|
|
#include "device.h"
|
|
#include "camera.h"
|
|
#include "pipeline.h"
|
|
#include "process_pipeline.h"
|
|
#include <string.h>
|
|
#include <glib.h>
|
|
#include <fcntl.h>
|
|
#include <sys/ioctl.h>
|
|
#include <errno.h>
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
|
|
struct media_link_info {
|
|
unsigned int source_entity_id;
|
|
unsigned int target_entity_id;
|
|
char source_fname[260];
|
|
char target_fname[260];
|
|
};
|
|
|
|
struct camera_info {
|
|
size_t device_index;
|
|
|
|
unsigned int pad_id;
|
|
|
|
char dev_fname[260];
|
|
int fd;
|
|
|
|
MPCamera *camera;
|
|
|
|
int gain_ctrl;
|
|
int gain_max;
|
|
|
|
bool has_auto_focus_continuous;
|
|
bool has_auto_focus_start;
|
|
|
|
|
|
// unsigned int entity_id;
|
|
// enum v4l2_buf_type type;
|
|
|
|
// char media_dev_fname[260];
|
|
// char video_dev_fname[260];
|
|
// int media_fd;
|
|
|
|
|
|
// struct mp_media_link media_links[MP_MAX_LINKS];
|
|
// int num_media_links;
|
|
|
|
// int gain_ctrl;
|
|
};
|
|
|
|
struct device_info {
|
|
const char *media_dev_name; // owned by camera config
|
|
|
|
MPDevice *device;
|
|
|
|
unsigned int interface_pad_id;
|
|
|
|
int video_fd;
|
|
};
|
|
|
|
static struct camera_info cameras[MP_MAX_CAMERAS];
|
|
|
|
static struct device_info devices[MP_MAX_CAMERAS];
|
|
static size_t num_devices = 0;
|
|
|
|
static const struct mp_camera_config *camera = NULL;
|
|
static MPCameraMode mode;
|
|
|
|
static bool just_switched_mode = false;
|
|
static int blank_frame_count = 0;
|
|
|
|
static int burst_length;
|
|
static int captures_remaining = 0;
|
|
|
|
static int preview_width;
|
|
static int preview_height;
|
|
|
|
struct control_state {
|
|
bool gain_is_manual;
|
|
int gain;
|
|
|
|
bool exposure_is_manual;
|
|
int exposure;
|
|
};
|
|
|
|
static struct control_state desired_controls = {};
|
|
static struct control_state current_controls = {};
|
|
|
|
static bool want_focus = false;
|
|
|
|
static MPPipeline *pipeline;
|
|
static GSource *capture_source;
|
|
|
|
static int
|
|
xioctl(int fd, int request, void *arg)
|
|
{
|
|
int r;
|
|
do {
|
|
r = ioctl(fd, request, arg);
|
|
} while (r == -1 && errno == EINTR);
|
|
return r;
|
|
}
|
|
|
|
static int
|
|
v4l2_ctrl_set(int fd, uint32_t id, int val)
|
|
{
|
|
struct v4l2_control ctrl = {0};
|
|
ctrl.id = id;
|
|
ctrl.value = val;
|
|
|
|
if (xioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) {
|
|
g_printerr("Failed to set control %d to %d\n", id, val);
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int
|
|
v4l2_ctrl_get(int fd, uint32_t id)
|
|
{
|
|
struct v4l2_control ctrl = {0};
|
|
ctrl.id = id;
|
|
|
|
if (xioctl(fd, VIDIOC_G_CTRL, &ctrl) == -1) {
|
|
g_printerr("Failed to get control %d\n", id);
|
|
return -1;
|
|
}
|
|
return ctrl.value;
|
|
}
|
|
|
|
static int
|
|
v4l2_ctrl_get_max(int fd, uint32_t id)
|
|
{
|
|
struct v4l2_queryctrl queryctrl;
|
|
int ret;
|
|
|
|
memset(&queryctrl, 0, sizeof(queryctrl));
|
|
|
|
queryctrl.id = id;
|
|
ret = xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl);
|
|
if (ret)
|
|
return 0;
|
|
|
|
if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
|
|
return 0;
|
|
}
|
|
|
|
return queryctrl.maximum;
|
|
}
|
|
|
|
static int
|
|
v4l2_has_control(int fd, int control_id)
|
|
{
|
|
struct v4l2_queryctrl queryctrl;
|
|
int ret;
|
|
|
|
memset(&queryctrl, 0, sizeof(queryctrl));
|
|
|
|
queryctrl.id = control_id;
|
|
ret = xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl);
|
|
if (ret)
|
|
return 0;
|
|
|
|
if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
static void setup_camera(MPDeviceList **device_list, const struct mp_camera_config *config)
|
|
{
|
|
// Find device info
|
|
size_t device_index = 0;
|
|
for (; device_index < num_devices; ++device_index) {
|
|
if (strcmp(config->media_dev_name, devices[device_index].media_dev_name) == 0) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (device_index == num_devices)
|
|
{
|
|
device_index = num_devices;
|
|
|
|
// Initialize new device
|
|
struct device_info *info = &devices[device_index];
|
|
info->media_dev_name = config->media_dev_name;
|
|
info->device = mp_device_list_find_remove(device_list, info->media_dev_name);
|
|
if (!info->device) {
|
|
g_printerr("Could not find /dev/media* node matching '%s'\n", info->media_dev_name);
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
const struct media_v2_entity *entity = mp_device_find_entity(info->device, info->media_dev_name);
|
|
if (!entity) {
|
|
g_printerr("Count not find device video entity\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
const struct media_v2_pad *pad = mp_device_get_pad_from_entity(info->device, entity->id);
|
|
info->interface_pad_id = pad->id;
|
|
|
|
const struct media_v2_interface *interface = mp_device_find_entity_interface(info->device, entity->id);
|
|
char dev_name[260];
|
|
if (!mp_find_device_path(interface->devnode, dev_name, 260)) {
|
|
g_printerr("Count not find video path\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
info->video_fd = open(dev_name, O_RDWR);
|
|
if (info->video_fd == -1) {
|
|
g_printerr("Could not open %s\n", dev_name);
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
++num_devices;
|
|
}
|
|
|
|
{
|
|
struct camera_info *info = &cameras[config->index];
|
|
struct device_info *dev_info = &devices[device_index];
|
|
|
|
info->device_index = device_index;
|
|
|
|
const struct media_v2_entity *entity = mp_device_find_entity(dev_info->device, config->dev_name);
|
|
if (!entity) {
|
|
g_printerr("Count not find camera entity matching '%s'\n", config->dev_name);
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
const struct media_v2_pad *pad = mp_device_get_pad_from_entity(dev_info->device, entity->id);
|
|
|
|
info->pad_id = pad->id;
|
|
|
|
// Make sure the camera starts out as disabled
|
|
mp_device_setup_link(
|
|
dev_info->device,
|
|
info->pad_id,
|
|
dev_info->interface_pad_id,
|
|
false);
|
|
|
|
const struct media_v2_interface *interface = mp_device_find_entity_interface(dev_info->device, entity->id);
|
|
|
|
if (!mp_find_device_path(interface->devnode, info->dev_fname, 260)) {
|
|
g_printerr("Count not find camera device path\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
info->fd = open(info->dev_fname, O_RDWR);
|
|
if (info->fd == -1) {
|
|
g_printerr("Could not open %s\n", info->dev_fname);
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
info->camera = mp_camera_new(dev_info->video_fd, info->fd);
|
|
|
|
// Trigger continuous auto focus if the sensor supports it
|
|
if (v4l2_has_control(info->fd, V4L2_CID_FOCUS_AUTO)) {
|
|
info->has_auto_focus_continuous = true;
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_FOCUS_AUTO, 1);
|
|
}
|
|
if (v4l2_has_control(info->fd, V4L2_CID_AUTO_FOCUS_START)) {
|
|
info->has_auto_focus_start = true;
|
|
}
|
|
|
|
if (v4l2_has_control(info->fd, V4L2_CID_GAIN)) {
|
|
info->gain_ctrl = V4L2_CID_GAIN;
|
|
info->gain_max = v4l2_ctrl_get_max(info->fd, V4L2_CID_GAIN);
|
|
}
|
|
|
|
if (v4l2_has_control(info->fd, V4L2_CID_ANALOGUE_GAIN)) {
|
|
info->gain_ctrl = V4L2_CID_ANALOGUE_GAIN;
|
|
info->gain_max = v4l2_ctrl_get_max(info->fd, V4L2_CID_ANALOGUE_GAIN);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void setup(MPPipeline *pipeline, const void *data)
|
|
{
|
|
MPDeviceList *device_list = mp_device_list_new();
|
|
|
|
for (size_t i = 0; i < MP_MAX_CAMERAS; ++i) {
|
|
const struct mp_camera_config *config = mp_get_camera_config(i);
|
|
if (!config) {
|
|
break;
|
|
}
|
|
|
|
setup_camera(&device_list, config);
|
|
}
|
|
|
|
mp_device_list_free(device_list);
|
|
}
|
|
|
|
void mp_io_pipeline_start()
|
|
{
|
|
mp_process_pipeline_start();
|
|
|
|
pipeline = mp_pipeline_new();
|
|
|
|
mp_pipeline_invoke(pipeline, setup, NULL, 0);
|
|
}
|
|
|
|
void mp_io_pipeline_stop()
|
|
{
|
|
if (capture_source) {
|
|
g_source_destroy(capture_source);
|
|
}
|
|
|
|
mp_pipeline_free(pipeline);
|
|
|
|
mp_process_pipeline_stop();
|
|
}
|
|
|
|
static void
|
|
update_process_pipeline()
|
|
{
|
|
struct camera_info *info = &cameras[camera->index];
|
|
|
|
// Grab the latest control values
|
|
if (!current_controls.gain_is_manual) {
|
|
current_controls.gain = v4l2_ctrl_get(info->fd, info->gain_ctrl);
|
|
}
|
|
if (!current_controls.exposure_is_manual) {
|
|
current_controls.exposure = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE);
|
|
}
|
|
|
|
struct mp_process_pipeline_state pipeline_state = {
|
|
.camera = camera,
|
|
.mode = mode,
|
|
.burst_length = burst_length,
|
|
.preview_width = preview_width,
|
|
.preview_height = preview_height,
|
|
.gain_is_manual = current_controls.gain_is_manual,
|
|
.gain = current_controls.gain,
|
|
.gain_max = info->gain_max,
|
|
.exposure_is_manual = current_controls.exposure_is_manual,
|
|
.exposure = current_controls.exposure,
|
|
.has_auto_focus_continuous = info->has_auto_focus_continuous,
|
|
.has_auto_focus_start = info->has_auto_focus_start,
|
|
};
|
|
mp_process_pipeline_update_state(&pipeline_state);
|
|
}
|
|
|
|
static void
|
|
focus(MPPipeline *pipeline, const void *data)
|
|
{
|
|
want_focus = true;
|
|
}
|
|
|
|
void mp_io_pipeline_focus()
|
|
{
|
|
mp_pipeline_invoke(pipeline, focus, NULL, 0);
|
|
}
|
|
|
|
static void
|
|
capture(MPPipeline *pipeline, const void *data)
|
|
{
|
|
struct camera_info *info = &cameras[camera->index];
|
|
|
|
captures_remaining = burst_length;
|
|
|
|
// Disable the autogain/exposure while taking the burst
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, 0);
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);
|
|
|
|
// Change camera mode for capturing
|
|
mp_camera_stop_capture(info->camera);
|
|
|
|
mode = camera->capture_mode;
|
|
mp_camera_set_mode(info->camera, &mode);
|
|
just_switched_mode = true;
|
|
|
|
mp_camera_start_capture(info->camera);
|
|
|
|
update_process_pipeline();
|
|
|
|
mp_process_pipeline_capture();
|
|
}
|
|
|
|
void mp_io_pipeline_capture()
|
|
{
|
|
mp_pipeline_invoke(pipeline, capture, NULL, 0);
|
|
}
|
|
|
|
static void
|
|
update_controls()
|
|
{
|
|
// Don't update controls while capturing
|
|
if (captures_remaining > 0) {
|
|
return;
|
|
}
|
|
|
|
struct camera_info *info = &cameras[camera->index];
|
|
|
|
if (want_focus) {
|
|
if (info->has_auto_focus_continuous) {
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_FOCUS_AUTO, 1);
|
|
} else if (info->has_auto_focus_start) {
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_AUTO_FOCUS_START, 1);
|
|
}
|
|
|
|
want_focus = false;
|
|
}
|
|
|
|
if (current_controls.gain_is_manual != desired_controls.gain_is_manual) {
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, desired_controls.gain_is_manual ? 0 : 1);
|
|
}
|
|
|
|
if (!desired_controls.gain_is_manual && current_controls.gain != desired_controls.gain) {
|
|
v4l2_ctrl_set(info->fd, info->gain_ctrl, desired_controls.gain);
|
|
}
|
|
|
|
if (current_controls.exposure_is_manual != desired_controls.exposure_is_manual) {
|
|
v4l2_ctrl_set(
|
|
info->fd,
|
|
V4L2_CID_EXPOSURE_AUTO,
|
|
desired_controls.exposure_is_manual ? V4L2_EXPOSURE_MANUAL : V4L2_EXPOSURE_AUTO);
|
|
}
|
|
|
|
if (!desired_controls.exposure_is_manual && current_controls.exposure != desired_controls.exposure) {
|
|
printf("Setting exposure to %d\n", desired_controls.exposure);
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE, desired_controls.exposure);
|
|
}
|
|
|
|
current_controls = desired_controls;
|
|
}
|
|
|
|
static void
|
|
on_frame(MPImage image, void *data)
|
|
{
|
|
// Only update controls right after a frame was captured
|
|
update_controls();
|
|
|
|
// When the mode is switched while capturing we get a couple blank frames,
|
|
// presumably from buffers made ready during the switch. Ignore these.
|
|
if (just_switched_mode)
|
|
{
|
|
if (blank_frame_count < 20) {
|
|
// Only check a 50x50 area
|
|
size_t test_size = MIN(50, image.width) * MIN(50, image.height);
|
|
|
|
bool image_is_blank = true;
|
|
for (size_t i = 0; i < test_size; ++i) {
|
|
if (image.data[i] != 0) {
|
|
image_is_blank = false;
|
|
}
|
|
}
|
|
|
|
if (image_is_blank) {
|
|
printf("Skipping blank image\n");
|
|
++blank_frame_count;
|
|
return;
|
|
}
|
|
} else {
|
|
printf("Blank image limit reached, resulting capture may be blank\n");
|
|
}
|
|
|
|
just_switched_mode = false;
|
|
blank_frame_count = 0;
|
|
}
|
|
|
|
// Copy from the camera buffer
|
|
size_t size = mp_pixel_format_width_to_bytes(image.pixel_format, image.width) * image.height;
|
|
uint8_t *buffer = malloc(size);
|
|
memcpy(buffer, image.data, size);
|
|
|
|
image.data = buffer;
|
|
|
|
// Send the image off for processing
|
|
mp_process_pipeline_process_image(image);
|
|
|
|
if (captures_remaining > 0) {
|
|
--captures_remaining;
|
|
|
|
if (captures_remaining == 0) {
|
|
struct camera_info *info = &cameras[camera->index];
|
|
|
|
// Restore the auto exposure and gain if needed
|
|
if (!current_controls.exposure_is_manual) {
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_AUTO);
|
|
}
|
|
|
|
if (!current_controls.gain_is_manual) {
|
|
v4l2_ctrl_set(info->fd, V4L2_CID_AUTOGAIN, 1);
|
|
}
|
|
|
|
// Go back to preview mode
|
|
mp_camera_stop_capture(info->camera);
|
|
|
|
mode = camera->preview_mode;
|
|
mp_camera_set_mode(info->camera, &mode);
|
|
just_switched_mode = true;
|
|
|
|
mp_camera_start_capture(info->camera);
|
|
|
|
update_process_pipeline();
|
|
}
|
|
}
|
|
}
|
|
|
|
static void
|
|
update_state(MPPipeline *pipeline, const struct mp_io_pipeline_state *state)
|
|
{
|
|
// Make sure the state isn't updated more than it needs to be by checking
|
|
// whether this state change actually changes anything.
|
|
bool has_changed = false;
|
|
|
|
if (camera != state->camera) {
|
|
has_changed = true;
|
|
|
|
if (camera) {
|
|
struct camera_info *info = &cameras[camera->index];
|
|
struct device_info *dev_info = &devices[info->device_index];
|
|
|
|
mp_camera_stop_capture(info->camera);
|
|
mp_device_setup_link(
|
|
dev_info->device,
|
|
info->pad_id,
|
|
dev_info->interface_pad_id,
|
|
false);
|
|
}
|
|
|
|
if (capture_source) {
|
|
g_source_destroy(capture_source);
|
|
capture_source = NULL;
|
|
}
|
|
|
|
camera = state->camera;
|
|
|
|
if (camera) {
|
|
struct camera_info *info = &cameras[camera->index];
|
|
struct device_info *dev_info = &devices[info->device_index];
|
|
|
|
mp_device_setup_link(
|
|
dev_info->device,
|
|
info->pad_id,
|
|
dev_info->interface_pad_id,
|
|
true);
|
|
|
|
mode = camera->preview_mode;
|
|
mp_camera_set_mode(info->camera, &mode);
|
|
|
|
mp_camera_start_capture(info->camera);
|
|
capture_source = mp_pipeline_add_capture_source(pipeline, info->camera, on_frame, NULL);
|
|
|
|
current_controls.gain_is_manual = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE_AUTO) == V4L2_EXPOSURE_MANUAL;
|
|
current_controls.gain = v4l2_ctrl_get(info->fd, info->gain_ctrl);
|
|
|
|
current_controls.exposure_is_manual = v4l2_ctrl_get(info->fd, V4L2_CID_AUTOGAIN) == 0;
|
|
current_controls.exposure = v4l2_ctrl_get(info->fd, V4L2_CID_EXPOSURE);
|
|
}
|
|
}
|
|
|
|
has_changed = has_changed
|
|
|| burst_length != state->burst_length
|
|
|| preview_width != state->preview_width
|
|
|| preview_height != state->preview_height;
|
|
|
|
burst_length = state->burst_length;
|
|
preview_width = state->preview_width;
|
|
preview_height = state->preview_height;
|
|
|
|
if (camera) {
|
|
struct control_state previous_desired = desired_controls;
|
|
|
|
desired_controls.gain_is_manual = state->gain_is_manual;
|
|
desired_controls.gain = state->gain;
|
|
desired_controls.exposure_is_manual = state->exposure_is_manual;
|
|
desired_controls.exposure = state->exposure;
|
|
|
|
has_changed = has_changed || memcmp(&previous_desired, &desired_controls, sizeof(struct control_state)) != 0;
|
|
}
|
|
|
|
assert(has_changed);
|
|
|
|
update_process_pipeline();
|
|
}
|
|
|
|
void mp_io_pipeline_update_state(const struct mp_io_pipeline_state *state)
|
|
{
|
|
mp_pipeline_invoke(pipeline, (MPPipelineCallback)update_state, state, sizeof(struct mp_io_pipeline_state));
|
|
}
|