sensorprofile.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478
  1. #include <libmegapixels.h>
  2. #include <stdio.h>
  3. #include <limits.h>
  4. #include <string.h>
  5. #include <linux/videodev2.h>
  6. #include <sys/ioctl.h>
  7. #include <stdlib.h>
  8. #include <sys/types.h>
  9. #include <errno.h>
  10. #include <sys/mman.h>
  11. #include <sys/time.h>
  12. #include <getopt.h>
  13. #include <ctype.h>
  14. #include <math.h>
  15. struct buffer {
  16. void *start;
  17. size_t length;
  18. };
  19. struct buffer *buffers;
  20. struct control {
  21. uint32_t id;
  22. int64_t min;
  23. int64_t max;
  24. uint64_t step;
  25. int64_t default_value;
  26. int32_t value;
  27. };
  28. int
  29. xioctl(int fd, int request, void *arg)
  30. {
  31. int r;
  32. do {
  33. r = ioctl(fd, request, arg);
  34. } while (r == -1 && errno == EINTR);
  35. return r;
  36. }
  37. void
  38. usage(char *name)
  39. {
  40. fprintf(stderr, "Usage: %s [-h] [-n offset] [-c camera] [-o file]\n", name);
  41. fprintf(stderr, "Measure the linearity of the sensor response\n\n");
  42. fprintf(stderr, "Arguments:\n");
  43. fprintf(stderr, " -n count Number of datapoint to gather, takes 1 second per point\n");
  44. fprintf(stderr, " -c camera Use a specific camera number\n");
  45. fprintf(stderr, " -m modenum Use another camera mode than the first\n");
  46. fprintf(stderr, " -o file File to store the calibration in\n");
  47. fprintf(stderr, " -h Display this help text\n");
  48. }
  49. void
  50. brightness(const uint8_t *buffer, size_t length, libmegapixels_mode *mode, float *red, float *green, float *blue)
  51. {
  52. // Get the offset to a single line of pixels at the middle of the frame
  53. size_t line = libmegapixels_mode_width_to_bytes(mode->format, mode->width);
  54. size_t stride = line + libmegapixels_mode_width_to_padding(mode->format, mode->width);
  55. size_t offset = stride * (mode->height / 2);
  56. unsigned long long sum_r = 0, sum_g = 0, sum_b = 0;
  57. unsigned int total = 0;
  58. for (size_t i = 0; i < line; i += 2) {
  59. uint8_t p1 = buffer[offset + i];
  60. uint8_t p2 = buffer[offset + i + 1];
  61. uint8_t p3 = buffer[offset + i + stride];
  62. //uint8_t p4 = buffer[offset + i + 1 + stride];
  63. total++;
  64. switch (mode->v4l_pixfmt) {
  65. case V4L2_PIX_FMT_SGRBG8:
  66. sum_r += p2;
  67. sum_g += p1;
  68. sum_b += p3;
  69. break;
  70. default:
  71. // TODO: Implement the other modes....
  72. fprintf(stderr, "Unsupported v4l pixfmt\n");
  73. exit(1);
  74. }
  75. }
  76. float max = (float) pow(2, libmegapixels_format_bits_per_pixel(mode->format)) - 1.0f;
  77. *red = (float) sum_r / (float) total / max;
  78. *green = (float) sum_g / (float) total / max;
  79. *blue = (float) sum_b / (float) total / max;
  80. }
  81. int
  82. get_control(int sensor_fd, struct control *control)
  83. {
  84. struct v4l2_ext_control ctrl = {};
  85. ctrl.id = control->id;
  86. struct v4l2_ext_controls ctrls = {
  87. .ctrl_class = 0,
  88. .which = V4L2_CTRL_WHICH_CUR_VAL,
  89. .count = 1,
  90. .controls = &ctrl,
  91. };
  92. if (xioctl(sensor_fd, VIDIOC_G_EXT_CTRLS, &ctrls) == -1) {
  93. if (errno != EINVAL) {
  94. fprintf(stderr, "VIDIOC_G_EXT_CTRLS\n");
  95. }
  96. return 0;
  97. }
  98. control->value = ctrl.value;
  99. return 1;
  100. }
  101. int
  102. set_control(int sensor_fd, struct control *control)
  103. {
  104. struct v4l2_ext_control ctrl = {};
  105. ctrl.id = control->id;
  106. ctrl.value = control->value;
  107. if (control->value > control->max || control->value < control->min) {
  108. fprintf(stderr, "Value %d is out of range for %ld..%ld\n", control->value, control->min, control->max);
  109. return 0;
  110. }
  111. struct v4l2_ext_controls ctrls = {
  112. .ctrl_class = 0,
  113. .which = V4L2_CTRL_WHICH_CUR_VAL,
  114. .count = 1,
  115. .controls = &ctrl,
  116. };
  117. if (xioctl(sensor_fd, VIDIOC_S_EXT_CTRLS, &ctrls) == -1) {
  118. if (errno != EINVAL) {
  119. fprintf(stderr, "VIDIOC_S_EXT_CTRLS\n");
  120. }
  121. return 0;
  122. }
  123. control->value = ctrl.value;
  124. return 1;
  125. }
  126. int
  127. query_control(int sensor_fd, struct control *control)
  128. {
  129. struct v4l2_query_ext_ctrl ctrl = {};
  130. ctrl.id = control->id;
  131. if (xioctl(sensor_fd, VIDIOC_QUERY_EXT_CTRL, &ctrl) == -1) {
  132. if (errno != EINVAL) {
  133. fprintf(stderr, "VIDIOC_QUERY_EXT_CTRL\n");
  134. }
  135. return 0;
  136. }
  137. control->min = ctrl.minimum;
  138. control->max = ctrl.maximum;
  139. control->step = ctrl.step;
  140. control->default_value = ctrl.default_value;
  141. return get_control(sensor_fd, control);
  142. }
  143. int
  144. main(int argc, char *argv[])
  145. {
  146. int c;
  147. int camera_id = 0;
  148. long res;
  149. char *end;
  150. char *outfile = NULL;
  151. int mode_idx = 0;
  152. int step = 0;
  153. int steps = 10;
  154. while ((c = getopt(argc, argv, "hc:n:o:m:")) != -1) {
  155. switch (c) {
  156. case 'c':
  157. res = strtol(optarg, &end, 10);
  158. if (end == optarg || end == NULL || *end != '\0') {
  159. fprintf(stderr, "Invalid number for -c\n");
  160. return 1;
  161. }
  162. camera_id = (int) res;
  163. break;
  164. case 'o':
  165. outfile = optarg;
  166. break;
  167. case 'm':
  168. res = strtol(optarg, &end, 10);
  169. if (end == optarg || end == NULL || *end != '\0') {
  170. fprintf(stderr, "Invalid number for -m\n");
  171. return 1;
  172. }
  173. mode_idx = (int) res;
  174. break;
  175. case 'n':
  176. res = strtol(optarg, &end, 10);
  177. if (end == optarg || end == NULL || *end != '\0') {
  178. fprintf(stderr, "Invalid number for -n\n");
  179. return 1;
  180. }
  181. steps = (int) res;
  182. break;
  183. case 'h':
  184. usage(argv[0]);
  185. return 0;
  186. break;
  187. case '?':
  188. if (optopt == 'd' || optopt == 'l') {
  189. fprintf(stderr, "Option -%c requires an argument.\n", optopt);
  190. } else if (isprint(optopt)) {
  191. fprintf(stderr, "Unknown option '-%c'\n", optopt);
  192. } else {
  193. fprintf(stderr, "Unknown option character x%x\n", optopt);
  194. }
  195. return 1;
  196. default:
  197. usage(argv[0]);
  198. return 1;
  199. }
  200. }
  201. char configpath[PATH_MAX];
  202. int ret = libmegapixels_find_config(configpath);
  203. libmegapixels_devconfig *config = {0};
  204. libmegapixels_init(&config);
  205. if (ret) {
  206. printf("Using config: %s\n", configpath);
  207. libmegapixels_load_file(config, configpath);
  208. } else {
  209. fprintf(stderr, "No config found for this device\n");
  210. }
  211. libmegapixels_load_uvc(config);
  212. if (config->count == 0) {
  213. fprintf(stderr, "No valid camera configuration\n");
  214. return 1;
  215. }
  216. if (camera_id > config->count - 1) {
  217. fprintf(stderr, "Camera id %d does not exist\n", camera_id);
  218. return 1;
  219. }
  220. libmegapixels_camera *camera = config->cameras[camera_id];
  221. if (libmegapixels_open(camera) != 0) {
  222. fprintf(stderr, "Could not open default camera\n");
  223. return 1;
  224. }
  225. if (mode_idx > camera->num_modes) {
  226. fprintf(stderr, "Invalid mode index: %d\n", mode_idx);
  227. }
  228. libmegapixels_mode *mode = camera->modes[mode_idx];
  229. struct v4l2_format format = {0};
  230. unsigned int frame_size = libmegapixels_select_mode(camera, mode, &format);
  231. if (frame_size == 0) {
  232. fprintf(stderr, "Could not select mode\n");
  233. return 1;
  234. }
  235. // Get the handles to the required V4L controls for the calibration
  236. struct control shutter = {.id = V4L2_CID_EXPOSURE};
  237. struct control gain = {.id = V4L2_CID_ANALOGUE_GAIN};
  238. if (!query_control(camera->sensor_fd, &shutter)) {
  239. fprintf(stderr, "Could not query V4L2_CID_EXPOSURE\n");
  240. return 1;
  241. }
  242. printf("Exposure: %ld..%ld step %lu val %d\n", shutter.min, shutter.max, shutter.step, shutter.value);
  243. if (!query_control(camera->sensor_fd, &gain)) {
  244. fprintf(stderr, "Could not query V4L2_CID_ANALOGUE_GAIN\n");
  245. return 1;
  246. }
  247. printf("Gain: %ld..%ld step %lu val %d\n", gain.min, gain.max, gain.step, gain.value);
  248. // Set the controls to the initial state
  249. shutter.value = shutter.max;
  250. if (!set_control(camera->sensor_fd, &shutter)) {
  251. fprintf(stderr, "Could not set the shutter to max\n");
  252. return 1;
  253. }
  254. // Do the reqular V4L2 stuff to get a frame
  255. struct v4l2_capability cap;
  256. int mplanes = 0;
  257. enum v4l2_buf_type buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  258. if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
  259. fprintf(stderr, "VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
  260. return 1;
  261. }
  262. if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
  263. fprintf(stderr, "Device does not support streaming i/o\n");
  264. return 1;
  265. }
  266. if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
  267. if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
  268. mplanes = 1;
  269. buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
  270. } else {
  271. fprintf(stderr, "Device does not support V4L2_CAP_VIDEO_CAPTURE\n");
  272. return 1;
  273. }
  274. }
  275. struct v4l2_requestbuffers req = {0};
  276. req.count = 4;
  277. req.type = buftype;
  278. req.memory = V4L2_MEMORY_MMAP;
  279. if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
  280. fprintf(stderr, "VIDIOC_REQBUFS failed: %s\n", strerror(errno));
  281. return 1;
  282. }
  283. buffers = calloc(req.count, sizeof(*buffers));
  284. for (int i = 0; i < req.count; i++) {
  285. struct v4l2_buffer buf = {0};
  286. buf.type = buftype;
  287. buf.memory = V4L2_MEMORY_MMAP;
  288. buf.index = i;
  289. struct v4l2_plane planes[1];
  290. if (mplanes) {
  291. buf.m.planes = planes;
  292. buf.length = 1;
  293. }
  294. if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
  295. fprintf(stderr, "VIDIOC_QUERYBUF failed: %s\n", strerror(errno));
  296. return 1;
  297. }
  298. unsigned int offset;
  299. if (mplanes) {
  300. buffers[i].length = planes[0].length;
  301. offset = planes[0].m.mem_offset;
  302. } else {
  303. buffers[i].length = buf.length;
  304. offset = buf.m.offset;
  305. }
  306. buffers[i].start = mmap(NULL, buffers[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, offset);
  307. if (buffers[i].start == MAP_FAILED) {
  308. fprintf(stderr, "mmap() failed\n");
  309. return 1;
  310. }
  311. }
  312. for (int i = 0; i < req.count; i++) {
  313. struct v4l2_buffer qbuf = {0};
  314. qbuf.type = buftype;
  315. qbuf.memory = V4L2_MEMORY_MMAP;
  316. qbuf.index = i;
  317. if (mplanes) {
  318. struct v4l2_plane qplanes[1];
  319. qbuf.m.planes = qplanes;
  320. qbuf.length = 1;
  321. }
  322. if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
  323. fprintf(stderr, "VIDIOC_QBUF failed: %s\n", strerror(errno));
  324. return 1;
  325. }
  326. }
  327. enum v4l2_buf_type type = buftype;
  328. if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
  329. fprintf(stderr, "VIDIOC_STREAMON failed: %s\n", strerror(errno));
  330. return 1;
  331. }
  332. // Open the target file
  333. FILE *outf = fopen(outfile, "w");
  334. if (outf == NULL) {
  335. fprintf(stderr, "Could not open output file\n");
  336. return 1;
  337. }
  338. printf("Performing initial setup...\n");
  339. struct timeval t_start, t_now;
  340. gettimeofday(&t_start, NULL);
  341. int stage = 1;
  342. double point = 1.0;
  343. while (stage > 0) {
  344. while (1) {
  345. fd_set(fds);
  346. FD_ZERO(&fds);
  347. FD_SET(camera->video_fd, &fds);
  348. int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL);
  349. if (sr == -1) {
  350. if (errno == EINTR) {
  351. continue;
  352. }
  353. fprintf(stderr, "select() failed: %s\n", strerror(errno));
  354. return 1;
  355. }
  356. struct v4l2_buffer buf = {0};
  357. buf.type = buftype;
  358. buf.memory = V4L2_MEMORY_MMAP;
  359. if (mplanes) {
  360. struct v4l2_plane dqplanes[1];
  361. buf.m.planes = dqplanes;
  362. buf.length = 1;
  363. }
  364. if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
  365. fprintf(stderr, "VIDIOC_DQBUF failed\n");
  366. return 1;
  367. }
  368. if (stage == 1) {
  369. // Setup stage to figure out initial brightness
  370. gettimeofday(&t_now, NULL);
  371. if (t_now.tv_sec - t_start.tv_sec > 1) {
  372. gettimeofday(&t_start, NULL);
  373. float red, green, blue;
  374. brightness(buffers[buf.index].start, buf.bytesused, mode, &red, &green, &blue);
  375. printf("Brightness: %f, %f, %f\n", red, green, blue);
  376. if (red == 1.0f || green == 1.0f || blue == 1.0f) {
  377. // Clipping the sensor. Lower gain
  378. if (gain.value == gain.min) {
  379. printf("! Lower the light source brightness, out of gain range\n");
  380. } else {
  381. gain.value -= gain.step;
  382. set_control(camera->sensor_fd, &gain);
  383. }
  384. } else if (red > 0.9 && green > 0.9 && blue > 0.9) {
  385. printf("Set up target hit, continue to calibration...\n\n\n");
  386. stage = 2;
  387. }
  388. }
  389. } else if (stage == 2) {
  390. gettimeofday(&t_now, NULL);
  391. if (t_now.tv_sec - t_start.tv_sec > 1) {
  392. gettimeofday(&t_start, NULL);
  393. float red, green, blue;
  394. brightness(buffers[buf.index].start, buf.bytesused, mode, &red, &green, &blue);
  395. printf("[%4d / %4d] %f: %f, %f, %f\n", step, steps, point, red, green, blue);
  396. fprintf(outf, "%f,%f,%f,%f\n", point, red, green, blue);
  397. step++;
  398. // Get next shutter value
  399. point = (double) step / (double) steps;
  400. point = 1.0 - point;
  401. uint32_t exposure = (uint32_t) (shutter.max * point);
  402. if (exposure < shutter.min) {
  403. exposure = shutter.min;
  404. }
  405. // Set the new shutter value for the next iteration
  406. shutter.value = exposure;
  407. set_control(camera->sensor_fd, &shutter);
  408. if (step == steps + 1) {
  409. stage = 0;
  410. fclose(outf);
  411. }
  412. }
  413. }
  414. if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
  415. fprintf(stderr, "VIDIOC_DQBUF failed\n");
  416. return 1;
  417. }
  418. break;
  419. }
  420. }
  421. return 0;
  422. }