start work on framerate

3 files changed, 122 insertions, 15 deletions

diff --git a/camera.c b/camera.c
index 0f164ce..368e9ec 100644
--- a/camera.c
+++ b/camera.c
@@ -27,12 +27,15 @@ struct Camera {
 	// this can be variable for compressed formats, and doesn't match v4l2_format sizeimage for grayscale for example
 	size_t frame_bytes_set;
 	bool any_frames;
+	bool streaming;
 	int curr_frame_idx;
 	int buffer_count;
 	struct v4l2_buffer frame_buffer;
 	CameraAccessMethod access_method;
 	PictureFormat best_format;
 	PictureFormat *formats;
+	// [i] = bitmask of frame rates supported for formats[i]
+	uint64_t *framerates_supported;
 	size_t mmap_size[CAMERA_MAX_BUFFERS];
 	uint8_t *mmap_frames[CAMERA_MAX_BUFFERS];
 	uint8_t *userp_frames[CAMERA_MAX_BUFFERS];
@@ -167,6 +170,7 @@ static bool camera_setup_with_read(Camera *camera) {
 	return camera->read_frame != NULL;
 }
 static bool camera_setup_with_mmap(Camera *camera) {
+	camera->streaming = true;
 	camera->access_method = CAMERA_ACCESS_MMAP;
 	struct v4l2_requestbuffers req = {0};
 	req.count = CAMERA_MAX_BUFFERS;
@@ -269,6 +273,7 @@ static bool camera_setup_with_userp(Camera *camera) {
 /*
 TODO: test me with a camera that supports userptr i/o
 	struct v4l2_requestbuffers req = {0};
+	camera->streaming = true;
 	req.count = CAMERA_MAX_BUFFERS;
 	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	req.memory = V4L2_MEMORY_USERPTR;
@@ -297,11 +302,14 @@ TODO: test me with a camera that supports userptr i/o
 	return true;*/
 }
 static bool camera_stop_io(Camera *camera) {
+	if (!camera->streaming)
+		return true;
 	camera->any_frames = false;
 	if (v4l2_ioctl(camera->fd, VIDIOC_STREAMOFF,
 		(enum v4l2_buf_type[1]) { V4L2_BUF_TYPE_VIDEO_CAPTURE }) != 0) {
 		perror("v4l2_ioctl VIDIOC_STREAMOFF");
 	}
+	camera->streaming = false;
 	// Just doing VIDIOC_STREAMOFF doesn't seem to be enough to prevent EBUSY.
 	//  (Even if we dequeue all buffers afterwards)
 	// Re-opening doesn't seem to be necessary for read-based access for me,
@@ -743,10 +751,44 @@ void camera_free(Camera *camera) {
 	camera_close(camera);
 	free(camera->devnode);
 	free(camera->name);
+	arr_free(camera->framerates_supported);
 	free(camera);
 }
 
-bool camera_set_format(Camera *camera, PictureFormat picfmt, CameraAccessMethod access, bool force) {
+uint64_t camera_framerates_supported(Camera *camera) {
+	ptrdiff_t format_idx = -1;
+	PictureFormat curr_pic_fmt = camera_picture_format(camera);
+	arr_foreach_ptr(camera->formats, PictureFormat, fmt) {
+		if (picture_format_cmp_qsort(fmt, &curr_pic_fmt) == 0) {
+			format_idx = fmt - camera->formats;
+			break;
+		}
+	}
+	if (format_idx >= 0) {
+		return camera->framerates_supported[format_idx];
+	}
+	// we can get here if we're letting V4L2 do the pixel format conversion for us
+	// take the AND of the supported framerates for all picture formats with this resolution.
+	uint64_t mask = UINT64_MAX;
+	arr_foreach_ptr(camera->formats, PictureFormat, fmt) {
+		if (picture_format_cmp_resolution(fmt, &curr_pic_fmt) == 0) {
+			mask &= camera->framerates_supported[fmt - camera->formats];
+		}
+	}
+	if (mask == 0 || mask == UINT64_MAX) {
+		// uhh let's hope 30FPS is supported.
+		return (uint64_t)1 << 30;
+	} else {
+		return mask;
+	}
+}
+
+bool camera_set_format(Camera *camera, PictureFormat picfmt, int desired_framerate, CameraAccessMethod access, bool force) {
+	assert(camera);
+	if (!access) {
+		// by default, don't change access method
+		access = camera->access_method;
+	}
 	if (!force
 		&& camera->access_method == access
 		&& picture_format_cmp_qsort((PictureFormat[1]) { camera_picture_format(camera) }, &picfmt) == 0) {
@@ -791,14 +833,51 @@ bool camera_set_format(Camera *camera, PictureFormat picfmt, CameraAccessMethod
 		return false;
 	}
 	camera->curr_format = format;
+
+	uint64_t framerates_supported = camera_framerates_supported(camera);
+	int framerate = 30;
+	if (desired_framerate) {
+		// select closest framerate to desired
+		for (int f = 63; f >= 0; f--) {
+			if (!(framerates_supported & ((uint64_t)1 << f))) continue;
+			if (abs(f - desired_framerate) < abs(framerate - desired_framerate)) {
+				framerate = f;
+			}
+		}
+	} else {
+		// select highest framerate
+		for (int f = 63; f >= 0; f--) {
+			if (framerates_supported & ((uint64_t)1 << f)) {
+				framerate = f;
+				break;
+			}
+		}
+	}
+	struct v4l2_streamparm stream_params = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.parm.capture = {.readbuffers = 4, .timeperframe = {1, (uint32_t)framerate}},
+	};
+	if (v4l2_ioctl(camera->fd, VIDIOC_S_PARM, &stream_params) != 0) {
+		perror("v4l2_ioctl VIDIOC_S_PARM");
+		// even if we don't get the framerate we want, don't fail.
+	}
+
 	//printf("image size = %uB\n",format.fmt.pix.sizeimage);
 	switch (camera->access_method) {
 	case CAMERA_ACCESS_READ:
 		return camera_setup_with_read(camera);
 	case CAMERA_ACCESS_MMAP:
-		return camera_setup_with_mmap(camera);
+		if (camera_setup_with_mmap(camera))
+			return true;
+		camera_stop_io(camera);
+		// try read instead
+		return camera_setup_with_read(camera);
 	case CAMERA_ACCESS_USERP:
-		return camera_setup_with_userp(camera);
+		if (camera_setup_with_userp(camera))
+			return true;
+		camera_stop_io(camera);
+		// try read instead
+		return camera_setup_with_read(camera);
 	default:
 		#if DEBUG
 		assert(false);
@@ -807,9 +886,9 @@ bool camera_set_format(Camera *camera, PictureFormat picfmt, CameraAccessMethod
 	}
 }
 
-bool camera_open(Camera *camera, PictureFormat desired_format) {
+bool camera_open(Camera *camera, PictureFormat desired_format, int desired_framerate) {
 	if (!camera->access_method)
-		camera->access_method = CAMERA_ACCESS_MMAP;
+		camera->access_method = CAMERA_ACCESS_MMAP; // by default
 	// camera should not already be open
 	assert(!camera->read_frame);
 	assert(!camera->mmap_frames[0]);
@@ -825,7 +904,7 @@ bool camera_open(Camera *camera, PictureFormat desired_format) {
 		camera_close(camera);
 		return false;
 	}
-	camera_set_format(camera, desired_format, camera->access_method, true);
+	camera_set_format(camera, desired_format, desired_framerate, camera->access_method, true);
 	return true;
 }
 
@@ -880,6 +959,25 @@ static void cameras_from_device_with_fd(const char *dev_path, const char *serial
 					.height = frame_height,
 					.pixfmt = fmtdesc.pixelformat,
 				}));
+				uint64_t framerates_supported = 0;
+				for (int i = 0; ; i++) {
+					struct v4l2_frmivalenum ival = {.index = i, .pixel_format = fmtdesc.pixelformat, .width = frame_width, .height = frame_height};
+					if (v4l2_ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &ival) != 0) {
+						if (errno != EINVAL) perror("v4l2_ioctl");
+						break;
+					}
+					// does anyone actually use continuous/stepwise? probably not.
+					struct v4l2_fract frmival = ival.type == V4L2_FRMIVAL_TYPE_DISCRETE ? ival.discrete : ival.stepwise.max;
+					if (frmival.numerator == 1 && frmival.denominator < 8*sizeof(framerates_supported)) {
+						framerates_supported |= (uint64_t)1 << frmival.denominator;
+					}
+				}
+				if (!framerates_supported) {
+					// assume 30FPS works
+					framerates_supported |= (uint64_t)1 << 30;
+				}
+				arr_add(camera->framerates_supported, framerates_supported);
+				
 			}
 		}
 		if (arr_len(camera->formats) == 0) {
diff --git a/camera.h b/camera.h
index 8a56e07..6e076d6 100644
--- a/camera.h
+++ b/camera.h
@@ -108,6 +108,7 @@ const char *pixfmt_to_string(uint32_t pixfmt);
 PictureFormat *camera_get_resolutions_with_pixfmt(Camera *camera, uint32_t pixfmt);
 uint32_t *camera_get_pixfmts(Camera *camera);
 PictureFormat camera_closest_picfmt(Camera *camera, PictureFormat picfmt);
+uint64_t camera_framerates_supported(Camera *camera);
 int32_t camera_frame_width(Camera *camera);
 int32_t camera_frame_height(Camera *camera);
 PictureFormat camera_picture_format(Camera *camera);
@@ -124,10 +125,10 @@ uint32_t camera_pixel_format(Camera *camera);
 CameraAccessMethod camera_access_method(Camera *camera);
 void camera_close(Camera *camera);
 void cameras_from_device(const char *dev_path, const char *serial, Camera ***cameras);
-bool camera_open(Camera *camera, PictureFormat desired_format);
+bool camera_open(Camera *camera, PictureFormat desired_format, int desired_framerate);
 Hash camera_hash(Camera *camera);
 void camera_hash_str(Camera *camera, char str[HASH_SIZE * 2 + 1]);
-bool camera_set_format(Camera *camera, PictureFormat picfmt, CameraAccessMethod access, bool force);
+bool camera_set_format(Camera *camera, PictureFormat picfmt, int desired_framerate, CameraAccessMethod access, bool force);
 /// Copy current frame from camera to AVFrame.
 ///
 /// Returns `true` on success. Currently only works if both the camera and the AVFrame are in the YUV420 format.
diff --git a/main.c b/main.c
index 80e7dde..d26bd76 100644
--- a/main.c
+++ b/main.c
@@ -83,6 +83,7 @@ typedef struct {
 	int timer;
 	int32_t image_resolution[2];
 	int32_t video_resolution[2];
+	int video_framerate;
 	ImageFormat image_format;
 	char *output_dir;
 } Settings;
@@ -141,12 +142,16 @@ static PictureFormat settings_desired_picture_format(State *state) {
 	};
 }
 
-
 static PictureFormat settings_picture_format_for_camera(State *state, Camera *camera) {
 	PictureFormat picfmt = settings_desired_picture_format(state);
 	return camera_closest_picfmt(camera, picfmt);
 }
 
+static int settings_desired_framerate(State *state) {
+	Settings *settings = &state->settings;
+	return state->mode == MODE_VIDEO ? settings->video_framerate : 0;
+}
+
 // compile a GLSL shader
 GLuint gl_compile_shader(char error_buf[256], const char *code, GLenum shader_type) {
 	GLuint shader = gl.CreateShader(shader_type);
@@ -364,7 +369,8 @@ static void menu_select(State *state) {
 		}
 		camera_set_format(state->camera,
 			resolution,
-			camera_access_method(state->camera),
+			settings_desired_framerate(state),
+			0,
 			false);
 		arr_free(resolutions);
 	} else if (state->curr_menu == MENU_INPUT) {
@@ -382,7 +388,7 @@ static void menu_select(State *state) {
 			camera_close(state->camera);
 			state->camera = new_camera;
 			PictureFormat picfmt = settings_picture_format_for_camera(state, state->camera);
-			if (camera_open(state->camera, picfmt)) {
+			if (camera_open(state->camera, picfmt, settings_desired_framerate(state))) {
 				// put at highest precedence
 				move_to_highest_precedence(&state->settings, state->camera);
 			} else {
@@ -403,7 +409,8 @@ static void menu_select(State *state) {
 		arr_free(pixfmts);
 		camera_set_format(state->camera,
 			new_picfmt,
-			camera_access_method(state->camera),
+			settings_desired_framerate(state),
+			0,
 			false);
 	} else if (state->curr_menu == MENU_HELP) {
 		state->curr_menu = 0;
@@ -449,7 +456,7 @@ static void select_camera(State *state) {
 			}
 			state->camera = state->cameras[camera_idx];
 		}
-		if (camera_open(state->camera, settings_picture_format_for_camera(state, state->camera))) {
+		if (camera_open(state->camera, settings_picture_format_for_camera(state, state->camera), settings_desired_framerate(state))) {
 			bool already_there = false;
 			arr_foreach_ptr(settings->camera_precedence, Hash, h) {
 				if (hash_eq(*h, camera_hash(state->camera))) {
@@ -1064,7 +1071,8 @@ void main() {\n\
 				case MODE_IMAGE:
 					camera_set_format(state->camera,
 						settings_picture_format_for_camera(state, state->camera),
-						camera_access_method(state->camera), false);
+						settings_desired_framerate(state),
+						0, false);
 					break;
 				case MODE_VIDEO: {
 					PictureFormat desired = settings_desired_picture_format(state);
@@ -1075,7 +1083,7 @@ void main() {\n\
 					PictureFormat picfmt = camera_closest_picfmt(state->camera, desired);
 					// force V4L2 to do the conversion if we have to
 					picfmt.pixfmt = V4L2_PIX_FMT_YUV420;
-					camera_set_format(state->camera, picfmt, camera_access_method(state->camera), false);
+					camera_set_format(state->camera, picfmt, settings_desired_framerate(state), 0, false);
 					} break;
 				case MODE_COUNT:
 					assert(false);