io_pipeline.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558
  1. #include "io_pipeline.h"
  2. #include "device.h"
  3. #include "camera.h"
  4. #include "pipeline.h"
  5. #include "process_pipeline.h"
  6. #include <string.h>
  7. #include <glib.h>
  8. #include <fcntl.h>
  9. #include <sys/ioctl.h>
  10. #include <errno.h>
  11. #include <assert.h>
  12. #include <stdio.h>
  13. struct media_link_info {
  14. unsigned int source_entity_id;
  15. unsigned int target_entity_id;
  16. char source_fname[260];
  17. char target_fname[260];
  18. };
  19. struct camera_info {
  20. size_t device_index;
  21. unsigned int pad_id;
  22. char dev_fname[260];
  23. int fd;
  24. MPCamera *camera;
  25. int gain_ctrl;
  26. int gain_max;
  27. bool has_auto_focus_continuous;
  28. bool has_auto_focus_start;
  29. // unsigned int entity_id;
  30. // enum v4l2_buf_type type;
  31. // char media_dev_fname[260];
  32. // char video_dev_fname[260];
  33. // int media_fd;
  34. // struct mp_media_link media_links[MP_MAX_LINKS];
  35. // int num_media_links;
  36. // int gain_ctrl;
  37. };
  38. struct device_info {
  39. const char *media_dev_name; // owned by camera config
  40. MPDevice *device;
  41. unsigned int interface_pad_id;
  42. int video_fd;
  43. };
  44. static struct camera_info cameras[MP_MAX_CAMERAS];
  45. static struct device_info devices[MP_MAX_CAMERAS];
  46. static size_t num_devices = 0;
  47. static const struct mp_camera_config *camera = NULL;
  48. static MPCameraMode mode;
  49. static bool just_switched_mode = false;
  50. static int blank_frame_count = 0;
  51. static int burst_length;
  52. static int captures_remaining = 0;
  53. static int preview_width;
  54. static int preview_height;
  55. static int device_rotation;
  56. struct control_state {
  57. bool gain_is_manual;
  58. int gain;
  59. bool exposure_is_manual;
  60. int exposure;
  61. };
  62. static struct control_state desired_controls = {};
  63. static struct control_state current_controls = {};
  64. static bool want_focus = false;
  65. static MPPipeline *pipeline;
  66. static GSource *capture_source;
  67. static void
  68. setup_camera(MPDeviceList **device_list, const struct mp_camera_config *config)
  69. {
  70. // Find device info
  71. size_t device_index = 0;
  72. for (; device_index < num_devices; ++device_index) {
  73. if (strcmp(config->media_dev_name,
  74. devices[device_index].media_dev_name) == 0) {
  75. break;
  76. }
  77. }
  78. if (device_index == num_devices) {
  79. device_index = num_devices;
  80. // Initialize new device
  81. struct device_info *info = &devices[device_index];
  82. info->media_dev_name = config->media_dev_name;
  83. info->device = mp_device_list_find_remove(device_list,
  84. info->media_dev_name);
  85. if (!info->device) {
  86. g_printerr("Could not find /dev/media* node matching '%s'\n",
  87. info->media_dev_name);
  88. exit(EXIT_FAILURE);
  89. }
  90. const struct media_v2_entity *entity =
  91. mp_device_find_entity_type(info->device, MEDIA_ENT_F_IO_V4L);
  92. if (!entity) {
  93. g_printerr("Could not find device video entity\n");
  94. exit(EXIT_FAILURE);
  95. }
  96. const struct media_v2_pad *pad =
  97. mp_device_get_pad_from_entity(info->device, entity->id);
  98. info->interface_pad_id = pad->id;
  99. const struct media_v2_interface *interface =
  100. mp_device_find_entity_interface(info->device, entity->id);
  101. char dev_name[260];
  102. if (!mp_find_device_path(interface->devnode, dev_name, 260)) {
  103. g_printerr("Could not find video path\n");
  104. exit(EXIT_FAILURE);
  105. }
  106. info->video_fd = open(dev_name, O_RDWR);
  107. if (info->video_fd == -1) {
  108. g_printerr("Could not open %s: %s\n", dev_name, strerror(errno));
  109. exit(EXIT_FAILURE);
  110. }
  111. ++num_devices;
  112. }
  113. {
  114. struct camera_info *info = &cameras[config->index];
  115. struct device_info *dev_info = &devices[device_index];
  116. info->device_index = device_index;
  117. const struct media_v2_entity *entity =
  118. mp_device_find_entity(dev_info->device, config->dev_name);
  119. if (!entity) {
  120. g_printerr("Could not find camera entity matching '%s'\n",
  121. config->dev_name);
  122. exit(EXIT_FAILURE);
  123. }
  124. const struct media_v2_pad *pad =
  125. mp_device_get_pad_from_entity(dev_info->device, entity->id);
  126. info->pad_id = pad->id;
  127. // Make sure the camera starts out as disabled
  128. mp_device_setup_link(dev_info->device, info->pad_id,
  129. dev_info->interface_pad_id, false);
  130. const struct media_v2_interface *interface =
  131. mp_device_find_entity_interface(dev_info->device,
  132. entity->id);
  133. if (!mp_find_device_path(interface->devnode, info->dev_fname, 260)) {
  134. g_printerr("Could not find camera device path\n");
  135. exit(EXIT_FAILURE);
  136. }
  137. info->fd = open(info->dev_fname, O_RDWR);
  138. if (info->fd == -1) {
  139. g_printerr("Could not open %s: %s\n", info->dev_fname, strerror(errno));
  140. exit(EXIT_FAILURE);
  141. }
  142. info->camera = mp_camera_new(dev_info->video_fd, info->fd);
  143. // Start with the capture format, this works around a bug with
  144. // the ov5640 driver where it won't allow setting the preview
  145. // format initially.
  146. MPCameraMode mode = config->capture_mode;
  147. mp_camera_set_mode(info->camera, &mode);
  148. // Trigger continuous auto focus if the sensor supports it
  149. if (mp_camera_query_control(info->camera, V4L2_CID_FOCUS_AUTO,
  150. NULL)) {
  151. info->has_auto_focus_continuous = true;
  152. mp_camera_control_set_bool(info->camera, V4L2_CID_FOCUS_AUTO,
  153. true);
  154. }
  155. if (mp_camera_query_control(info->camera, V4L2_CID_AUTO_FOCUS_START,
  156. NULL)) {
  157. info->has_auto_focus_start = true;
  158. }
  159. MPControl control;
  160. if (mp_camera_query_control(info->camera, V4L2_CID_GAIN, &control)) {
  161. info->gain_ctrl = V4L2_CID_GAIN;
  162. info->gain_max = control.max;
  163. } else if (mp_camera_query_control(
  164. info->camera, V4L2_CID_ANALOGUE_GAIN, &control)) {
  165. info->gain_ctrl = V4L2_CID_ANALOGUE_GAIN;
  166. info->gain_max = control.max;
  167. }
  168. }
  169. }
  170. static void
  171. setup(MPPipeline *pipeline, const void *data)
  172. {
  173. MPDeviceList *device_list = mp_device_list_new();
  174. for (size_t i = 0; i < MP_MAX_CAMERAS; ++i) {
  175. const struct mp_camera_config *config = mp_get_camera_config(i);
  176. if (!config) {
  177. break;
  178. }
  179. setup_camera(&device_list, config);
  180. }
  181. mp_device_list_free(device_list);
  182. }
  183. void
  184. mp_io_pipeline_start()
  185. {
  186. mp_process_pipeline_start();
  187. pipeline = mp_pipeline_new();
  188. mp_pipeline_invoke(pipeline, setup, NULL, 0);
  189. }
  190. void
  191. mp_io_pipeline_stop()
  192. {
  193. if (capture_source) {
  194. g_source_destroy(capture_source);
  195. }
  196. mp_pipeline_free(pipeline);
  197. mp_process_pipeline_stop();
  198. }
  199. static void
  200. update_process_pipeline()
  201. {
  202. struct camera_info *info = &cameras[camera->index];
  203. // Grab the latest control values
  204. if (!current_controls.gain_is_manual) {
  205. current_controls.gain =
  206. mp_camera_control_get_int32(info->camera, info->gain_ctrl);
  207. }
  208. if (!current_controls.exposure_is_manual) {
  209. current_controls.exposure =
  210. mp_camera_control_get_int32(info->camera, V4L2_CID_EXPOSURE);
  211. }
  212. struct mp_process_pipeline_state pipeline_state = {
  213. .camera = camera,
  214. .mode = mode,
  215. .burst_length = burst_length,
  216. .preview_width = preview_width,
  217. .preview_height = preview_height,
  218. .device_rotation = device_rotation,
  219. .gain_is_manual = current_controls.gain_is_manual,
  220. .gain = current_controls.gain,
  221. .gain_max = info->gain_max,
  222. .exposure_is_manual = current_controls.exposure_is_manual,
  223. .exposure = current_controls.exposure,
  224. .has_auto_focus_continuous = info->has_auto_focus_continuous,
  225. .has_auto_focus_start = info->has_auto_focus_start,
  226. };
  227. mp_process_pipeline_update_state(&pipeline_state);
  228. }
  229. static void
  230. focus(MPPipeline *pipeline, const void *data)
  231. {
  232. want_focus = true;
  233. }
  234. void
  235. mp_io_pipeline_focus()
  236. {
  237. mp_pipeline_invoke(pipeline, focus, NULL, 0);
  238. }
  239. static void
  240. capture(MPPipeline *pipeline, const void *data)
  241. {
  242. struct camera_info *info = &cameras[camera->index];
  243. captures_remaining = burst_length;
  244. // Disable the autogain/exposure while taking the burst
  245. mp_camera_control_set_int32(info->camera, V4L2_CID_AUTOGAIN, 0);
  246. mp_camera_control_set_int32(info->camera, V4L2_CID_EXPOSURE_AUTO,
  247. V4L2_EXPOSURE_MANUAL);
  248. // Change camera mode for capturing
  249. mp_process_pipeline_sync();
  250. mp_camera_stop_capture(info->camera);
  251. mode = camera->capture_mode;
  252. mp_camera_set_mode(info->camera, &mode);
  253. just_switched_mode = true;
  254. mp_camera_start_capture(info->camera);
  255. update_process_pipeline();
  256. mp_process_pipeline_capture();
  257. }
  258. void
  259. mp_io_pipeline_capture()
  260. {
  261. mp_pipeline_invoke(pipeline, capture, NULL, 0);
  262. }
  263. static void
  264. release_buffer(MPPipeline *pipeline, const uint32_t *buffer_index)
  265. {
  266. struct camera_info *info = &cameras[camera->index];
  267. mp_camera_release_buffer(info->camera, *buffer_index);
  268. }
  269. void mp_io_pipeline_release_buffer(uint32_t buffer_index)
  270. {
  271. mp_pipeline_invoke(pipeline, (MPPipelineCallback) release_buffer, &buffer_index, sizeof(uint32_t));
  272. }
  273. static void
  274. update_controls()
  275. {
  276. // Don't update controls while capturing
  277. if (captures_remaining > 0) {
  278. return;
  279. }
  280. struct camera_info *info = &cameras[camera->index];
  281. if (want_focus) {
  282. if (info->has_auto_focus_continuous) {
  283. mp_camera_control_set_bool(info->camera, V4L2_CID_FOCUS_AUTO,
  284. 1);
  285. } else if (info->has_auto_focus_start) {
  286. mp_camera_control_set_bool(info->camera,
  287. V4L2_CID_AUTO_FOCUS_START, 1);
  288. }
  289. want_focus = false;
  290. }
  291. if (current_controls.gain_is_manual != desired_controls.gain_is_manual) {
  292. mp_camera_control_set_bool(info->camera, V4L2_CID_AUTOGAIN,
  293. !desired_controls.gain_is_manual);
  294. }
  295. if (desired_controls.gain_is_manual &&
  296. current_controls.gain != desired_controls.gain) {
  297. mp_camera_control_set_int32(info->camera, info->gain_ctrl,
  298. desired_controls.gain);
  299. }
  300. if (current_controls.exposure_is_manual !=
  301. desired_controls.exposure_is_manual) {
  302. mp_camera_control_set_int32(info->camera, V4L2_CID_EXPOSURE_AUTO,
  303. desired_controls.exposure_is_manual ?
  304. V4L2_EXPOSURE_MANUAL :
  305. V4L2_EXPOSURE_AUTO);
  306. }
  307. if (desired_controls.exposure_is_manual &&
  308. current_controls.exposure != desired_controls.exposure) {
  309. mp_camera_control_set_int32(info->camera, V4L2_CID_EXPOSURE,
  310. desired_controls.exposure);
  311. }
  312. current_controls = desired_controls;
  313. }
  314. static void
  315. on_frame(MPBuffer buffer, void * _data)
  316. {
  317. // Only update controls right after a frame was captured
  318. update_controls();
  319. // When the mode is switched while capturing we get a couple blank frames,
  320. // presumably from buffers made ready during the switch. Ignore these.
  321. if (just_switched_mode) {
  322. if (blank_frame_count < 20) {
  323. // Only check a 10x10 area
  324. size_t test_size =
  325. MIN(10, mode.width) * MIN(10, mode.height);
  326. bool image_is_blank = true;
  327. for (size_t i = 0; i < test_size; ++i) {
  328. if (buffer.data[i] != 0) {
  329. image_is_blank = false;
  330. }
  331. }
  332. if (image_is_blank) {
  333. ++blank_frame_count;
  334. return;
  335. }
  336. } else {
  337. printf("Blank image limit reached, resulting capture may be blank\n");
  338. }
  339. just_switched_mode = false;
  340. blank_frame_count = 0;
  341. }
  342. // Send the image off for processing
  343. mp_process_pipeline_process_image(buffer);
  344. if (captures_remaining > 0) {
  345. --captures_remaining;
  346. if (captures_remaining == 0) {
  347. struct camera_info *info = &cameras[camera->index];
  348. // Restore the auto exposure and gain if needed
  349. if (!current_controls.exposure_is_manual) {
  350. mp_camera_control_set_int32(info->camera,
  351. V4L2_CID_EXPOSURE_AUTO,
  352. V4L2_EXPOSURE_AUTO);
  353. }
  354. if (!current_controls.gain_is_manual) {
  355. mp_camera_control_set_bool(info->camera,
  356. V4L2_CID_AUTOGAIN, true);
  357. }
  358. // Go back to preview mode
  359. mp_process_pipeline_sync();
  360. mp_camera_stop_capture(info->camera);
  361. mode = camera->preview_mode;
  362. mp_camera_set_mode(info->camera, &mode);
  363. just_switched_mode = true;
  364. mp_camera_start_capture(info->camera);
  365. update_process_pipeline();
  366. }
  367. }
  368. }
  369. static void
  370. update_state(MPPipeline *pipeline, const struct mp_io_pipeline_state *state)
  371. {
  372. // Make sure the state isn't updated more than it needs to be by checking
  373. // whether this state change actually changes anything.
  374. bool has_changed = false;
  375. if (camera != state->camera) {
  376. has_changed = true;
  377. if (camera) {
  378. struct camera_info *info = &cameras[camera->index];
  379. struct device_info *dev_info = &devices[info->device_index];
  380. mp_process_pipeline_sync();
  381. mp_camera_stop_capture(info->camera);
  382. mp_device_setup_link(dev_info->device, info->pad_id,
  383. dev_info->interface_pad_id, false);
  384. }
  385. if (capture_source) {
  386. g_source_destroy(capture_source);
  387. capture_source = NULL;
  388. }
  389. camera = state->camera;
  390. if (camera) {
  391. struct camera_info *info = &cameras[camera->index];
  392. struct device_info *dev_info = &devices[info->device_index];
  393. mp_device_setup_link(dev_info->device, info->pad_id,
  394. dev_info->interface_pad_id, true);
  395. mode = camera->preview_mode;
  396. mp_camera_set_mode(info->camera, &mode);
  397. mp_camera_start_capture(info->camera);
  398. capture_source = mp_pipeline_add_capture_source(
  399. pipeline, info->camera, on_frame, NULL);
  400. current_controls.gain_is_manual =
  401. mp_camera_control_get_bool(info->camera,
  402. V4L2_CID_AUTOGAIN) == 0;
  403. current_controls.gain = mp_camera_control_get_int32(
  404. info->camera, info->gain_ctrl);
  405. current_controls.exposure_is_manual =
  406. mp_camera_control_get_int32(
  407. info->camera, V4L2_CID_EXPOSURE_AUTO) ==
  408. V4L2_EXPOSURE_MANUAL;
  409. current_controls.exposure = mp_camera_control_get_int32(
  410. info->camera, V4L2_CID_EXPOSURE);
  411. }
  412. }
  413. has_changed = has_changed || burst_length != state->burst_length ||
  414. preview_width != state->preview_width ||
  415. preview_height != state->preview_height ||
  416. device_rotation != state->device_rotation;
  417. burst_length = state->burst_length;
  418. preview_width = state->preview_width;
  419. preview_height = state->preview_height;
  420. device_rotation = state->device_rotation;
  421. if (camera) {
  422. struct control_state previous_desired = desired_controls;
  423. desired_controls.gain_is_manual = state->gain_is_manual;
  424. desired_controls.gain = state->gain;
  425. desired_controls.exposure_is_manual = state->exposure_is_manual;
  426. desired_controls.exposure = state->exposure;
  427. has_changed =
  428. has_changed || memcmp(&previous_desired, &desired_controls,
  429. sizeof(struct control_state)) != 0;
  430. }
  431. assert(has_changed);
  432. update_process_pipeline();
  433. }
  434. void
  435. mp_io_pipeline_update_state(const struct mp_io_pipeline_state *state)
  436. {
  437. mp_pipeline_invoke(pipeline, (MPPipelineCallback)update_state, state,
  438. sizeof(struct mp_io_pipeline_state));
  439. }