From a5e7e6f616fe301210fea33ec7ff88b7eca60bdb Mon Sep 17 00:00:00 2001 From: jonas Date: Mon, 12 Oct 2020 08:58:53 +0200 Subject: Simple python application that uses the O-3000 driver and Color Image Pipeline using a Python C-Extension. --- helpers.c | 407 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 407 insertions(+) create mode 100644 helpers.c (limited to 'helpers.c') diff --git a/helpers.c b/helpers.c new file mode 100644 index 0000000..533769d --- /dev/null +++ b/helpers.c @@ -0,0 +1,407 @@ +/* +Copyright 2019 - Stettbacher Signal Processing AG + +Author: Patrick Roth + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or other +materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include "helpers.h" + + + +/** + * Save image as JPEG format + * + * @param filename filename as string + * @param format save image with this format + * @param color_pipe piped out image data + * @param compress 0 if no compression is used + * @return 0 on success, -1 on error + */ +static int save_jpeg(char *filename, enum frame_format_t format, struct color_pipe_t *__restrict__ color_pipe, int compress_img) { + printf("%s: saving JPEG image failed --> not implemented yet!!\n", __func__); + return -1; +} + + +/** + * Save image with TIFF format + * + * @param filename filename as string + * @param format save image with this format + * @param color_pipe piped out image data + * @param compress 0 if no compression is used + * @return 0 on success, -1 on error + */ +static int save_tiff(char *filename, enum frame_format_t format, struct color_pipe_t *__restrict__ color_pipe, int compress_img) { + + TIFF *tif; + int samples_per_pixel; + int i, u, height, width; + uint8_t *in_img8 = color_pipe->img_out; + uint16_t *in_img16 = color_pipe->img_out; + uint16_t *linebuf16; + int bitpersample; + + + if(color_pipe->is_color) { + samples_per_pixel = 3; + } + else { + samples_per_pixel = 1; + } + height = color_pipe->height; + width = color_pipe->width; + + if(color_pipe->bit_channel == 8) { + bitpersample = 8; + } + else if(color_pipe->bit_channel == 12) { + bitpersample = 16; + } + else { + printf("%s: %d bits per channel is not supported yet\n", __func__, color_pipe->bit_channel); + return -1; + } + + tif = TIFFOpen(filename,"w"); + TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, width); + TIFFSetField(tif, TIFFTAG_IMAGELENGTH, height); + TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, samples_per_pixel); + TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, bitpersample); + TIFFSetField(tif, TIFFTAG_ORIENTATION, ORIENTATION_TOPLEFT); + TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP, width*samples_per_pixel); + TIFFSetField(tif, TIFFTAG_XRESOLUTION, (float)width); + TIFFSetField(tif, TIFFTAG_YRESOLUTION, (float)height); + TIFFSetField(tif, TIFFTAG_RESOLUTIONUNIT, RESUNIT_NONE); + TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, 0); + TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG); + if(compress_img) { + TIFFSetField(tif, TIFFTAG_COMPRESSION, COMPRESSION_LZW); + } + else { + TIFFSetField(tif, TIFFTAG_COMPRESSION, COMPRESSION_NONE); + } + TIFFSetField(tif, TIFFTAG_FILLORDER, FILLORDER_MSB2LSB); + + if(color_pipe->is_color) { + TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_RGB); + } + else { + TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK); + } + + if(bitpersample == 8) { + // 8 bit per channel RGB image + for(i = 0; i < height; i++) { + TIFFWriteScanline(tif, (void*)&(in_img8[i*width*samples_per_pixel]), i, 0); + } + } + else if(bitpersample == 16) { + // 12 bit per channel RGB image (upscaling from 12 to 16 bit needed) + linebuf16 = malloc(width*samples_per_pixel*2); + if(linebuf16 == NULL) { + printf("%s: allocating memory failed: %s\n", __func__, strerror(errno)); + goto _end_savetiff; + } + + for(i = 0; i < height; i++) { + // upscale 12 to 16 bit + for(u = 0; u < width*samples_per_pixel; u++) { + linebuf16[u] = in_img16[i*width*samples_per_pixel+u] << 4; + } + TIFFWriteScanline(tif, (void*)linebuf16, i, 0); + } + free(linebuf16); + } + +_end_savetiff: + TIFFClose(tif); + return 0; +} + + +/** + * Save image frame with given file format. + * + * @param filename filename as string + * @param format save image with this format + * @param color_pipe piped out image data + * @param compress 0 if no compression is used + * @return 0 on success, -1 on error + */ +int save_imgframe(char *filename, enum frame_format_t format, struct color_pipe_t *color_pipe, int compress) { + + char filename_ext[256]; + + switch(format) { + case IMGFRAME_FORMAT_TIF: + sprintf(filename_ext, "%s.tif", filename); + save_tiff(filename_ext, format, color_pipe, compress); + break; + + case IMGFRAME_FORMAT_JPEG: + sprintf(filename_ext, "%s.jpeg", filename); + save_jpeg(filename_ext, format, color_pipe, compress); + break; + + default: + printf("%s: saving image with format %d not implemeted yet\n", __func__, format); + return -1; + } + return 0; +} + + +/** + * Tokenise (split) string + * + * This function uses strtok_r and therefore it's thread safe. + * + * @param s input string + * @param argv On return: array of string pointers containing the substrings + * @param argv_len string array length + * @param delim string delimiter used to tokenize input string ('\0' terminated) + * @return number of substrings splitted or -1 on error + */ + +int tokenise(char *s, char **argv, int argv_len, char *delim) { + + char *str; + int argc=0, i; + char *save_ptr; + + for(i = 0; i < argv_len; i++) { + argv[i] = NULL; + } + + str = strtok_r(s, delim, &save_ptr); + + while((str != NULL) && (argc < argv_len)) { + argv[argc++] = str; + str = strtok_r(NULL,delim,&save_ptr); + } + + if(argc > argv_len) { + return -1; + } + return argc; +} + + + +/** + * generic function to start thread + * This function is used to start a new thread. Normally all threads are scheduled with real time policy. If the + * priority is defined as @ref PRIO_TIME_SLICED a normal scheduling policy is used (no real time). + * + * @param thr On return: The ID of the new thread. + * @param func The start function. + * @param prio The thread priority. + * @param param The thread parameters. + * @return 0 on success otherwis -1. + */ +int generic_start_thread(pthread_t *thr, void*(*func)(void*), int prio, void *param) { + int r; + pthread_attr_t attr; + sigset_t bmask, oldmask; + struct sched_param sp; + + r = pthread_attr_init(&attr); // create attribute object + if(r) { + printf("%s: pthread_attr_init failed: %s\n", __func__, strerror(r)); + return -1; + } + + if(prio == PRIO_TIME_SLICED) { + r = pthread_attr_setschedpolicy(&attr, SCHED_OTHER); + sp.sched_priority = 0; + } + else { + r = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); // set realtime scheduling policy + sp.sched_priority = sched_get_priority_max(SCHED_FIFO) - prio; + } + if(r) { + printf("%s: pthread_attr_setschedpolicy failed: %s\n", __func__, strerror(r)); + goto _generic_start_thread_abort; + } + r = pthread_attr_setschedparam(&attr, &sp); + if(r) { + printf("%s: pthread_attr_setschedparam failed: %s\n", __func__, strerror(r)); + goto _generic_start_thread_abort; + } + r = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if(r) { + printf("%s: pthread_attr_setinheritsched failed: %s\n", __func__, strerror(r)); + goto _generic_start_thread_abort; + } + + //temporarely block some signals and inherit the signal mask to the new thread + if(sigemptyset(&bmask)) { + printf("%s: sigemptyset failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGTERM)) { + printf("%s: sigaddset (SIGTERM) failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGINT)) { + printf("%s: sigaddset (SIGINT) failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGALRM)) { + printf("%s: sigaddset (SIGALRM) \n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGCHLD)) { + printf("%s: sigaddset (SIGCHLD) failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGUSR1)) { + printf("%s: sigaddset (SIGUSR1) failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigaddset(&bmask, SIGUSR2)) { + printf("%s: sigaddset (SIGUSR2) failed\n", __func__); + goto _generic_start_thread_abort; + } + if(sigprocmask(SIG_BLOCK, &bmask, &oldmask)) { + printf("%s: sigprocmask failed: %s\n", __func__, strerror(errno)); + goto _generic_start_thread_abort; + } + + //create thread + r = pthread_create(thr, &attr, func, param); + if(r) { + printf("%s: pthread_create failed: %s\n", __func__, strerror(r)); + goto _generic_start_thread_abort; + } + + //restore old signalmask + if(sigprocmask (SIG_SETMASK, &oldmask, NULL)) { + printf("%s: sigprocmask failed: %s\n", __func__, strerror(errno)); + goto _generic_start_thread_abort; + } + + pthread_attr_destroy(&attr); //release attribute object + return 0; + +_generic_start_thread_abort: + pthread_attr_destroy(&attr); // release attribute object + return -1; +} + + +/** + * Execute a shell command. + * This function is thread safe. + * + * @param cmd shell command to be executed + * @param retstr On return: The return string from shell command execution. If NULL is defined nothing is returned. + * @param len maximum length of return string + * @return -1 if an error is detected + */ +int exec_shell_command(char *cmd, char *retstr, int len) { + FILE *substream; + char buf[512]; + int ret, len_tmp; + + if(retstr == NULL) { + ret = system(cmd); + } + else { + substream = popen(cmd, "r"); // start subprocess with streampipe connection + if (substream == NULL) { + printf("%s: popen failed: %s\n", __func__, strerror(errno)); + return -1; + } + + if(setvbuf(substream, NULL, _IOLBF, 0)) { // set line buffering for the stream + printf("%s: setvbuf failed: %s\n", __func__, strerror(errno)); + return -1; + } + + if (len > 0) { + retstr[0] = '\0'; + } + while(fgets(buf, sizeof(buf), substream) != NULL) { + len_tmp = strlen(buf); + if(len > 0) { + strncpy(retstr, buf, len); + retstr[len-1] = '\0'; + retstr += len_tmp; + len -= len_tmp; + } + } + ret = pclose(substream); + } + return ret; +} + + +/** + * Set the given process name to realtime scheduling with the definded priority. + * 0 means highest priority. + * + * NOTE + * Time critical scheduling policy SCHED_RR (real-time round-robin) is used! + * + * @param p_name process name (name given command 'ps') + * @param prio priority + * @return 0 on success, -1 on error + */ +int setRealtimeScheduling(char *p_name, int prio) { + + char shell_cmd[256]; + int pid; + char str[256]; + struct sched_param sp; + + // check string length of given process name + if(strlen(p_name) > (sizeof(shell_cmd)-30)) { + printf("%s: process name is too long\n", __func__); + return -1; + } + + sprintf(shell_cmd, "pidof %s", p_name); + if(exec_shell_command(shell_cmd, str, sizeof(str))) { + printf("%s: Setting realtime scheduling for '%s' failed: '%s'\n", __func__, p_name, str); + return -1; + } + + pid = atoi(str); + sp.sched_priority = sched_get_priority_max(SCHED_RR) - prio; + if(sched_setscheduler(pid, SCHED_RR, &sp)) { + printf("%s: sched_setscheduler failed: %s\n", __func__, strerror(errno)); + return -1; + } + return 0; +} -- cgit v1.2.1