diff --git a/conf/modules/cv_blob_locator.xml b/conf/modules/cv_blob_locator.xml index 90667f127c..a8453d2b29 100644 --- a/conf/modules/cv_blob_locator.xml +++ b/conf/modules/cv_blob_locator.xml @@ -5,6 +5,7 @@ Find a colored item and track its geo-location and update a waypoint to it + diff --git a/conf/modules/cv_colorfilter.xml b/conf/modules/cv_colorfilter.xml index 1c1ef97fb0..4cd98fd1fb 100644 --- a/conf/modules/cv_colorfilter.xml +++ b/conf/modules/cv_colorfilter.xml @@ -4,6 +4,7 @@ ColorFilter + diff --git a/conf/modules/cv_detect_contour.xml b/conf/modules/cv_detect_contour.xml index aecad43302..8dacd055ef 100644 --- a/conf/modules/cv_detect_contour.xml +++ b/conf/modules/cv_detect_contour.xml @@ -6,6 +6,7 @@ +
diff --git a/conf/modules/cv_detect_window.xml b/conf/modules/cv_detect_window.xml index b96726354d..6ecdd92a7c 100644 --- a/conf/modules/cv_detect_window.xml +++ b/conf/modules/cv_detect_window.xml @@ -5,6 +5,8 @@ Detect window + +
diff --git a/conf/modules/cv_opencvdemo.xml b/conf/modules/cv_opencvdemo.xml index 27469ced5b..7a8eee0d48 100644 --- a/conf/modules/cv_opencvdemo.xml +++ b/conf/modules/cv_opencvdemo.xml @@ -9,6 +9,7 @@ +
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 8b61de897d..b6625bcb26 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -37,6 +37,7 @@ + diff --git a/conf/modules/cv_qrcode.xml b/conf/modules/cv_qrcode.xml index dd97c15bf6..5b1c7bd749 100644 --- a/conf/modules/cv_qrcode.xml +++ b/conf/modules/cv_qrcode.xml @@ -8,6 +8,7 @@ A telemetry message with the code content is sent when a QR code is detected when qrscan is called. + diff --git a/conf/modules/video_capture.xml b/conf/modules/video_capture.xml index 23450d25a9..be32dd7338 100644 --- a/conf/modules/video_capture.xml +++ b/conf/modules/video_capture.xml @@ -12,6 +12,7 @@ + diff --git a/conf/modules/video_usb_logger.xml b/conf/modules/video_usb_logger.xml index 54d76e4c35..5ad91d947a 100644 --- a/conf/modules/video_usb_logger.xml +++ b/conf/modules/video_usb_logger.xml @@ -11,6 +11,7 @@ + video_thread,pose_history
diff --git a/sw/airborne/modules/computer_vision/colorfilter.c b/sw/airborne/modules/computer_vision/colorfilter.c index 501247499b..3b0088424c 100644 --- a/sw/airborne/modules/computer_vision/colorfilter.c +++ b/sw/airborne/modules/computer_vision/colorfilter.c @@ -58,5 +58,5 @@ struct image_t *colorfilter_func(struct image_t *img) void colorfilter_init(void) { - listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func); -} \ No newline at end of file + listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS); +} diff --git a/sw/airborne/modules/computer_vision/cv.c b/sw/airborne/modules/computer_vision/cv.c index 5c347ee15d..b829122f2b 100644 --- a/sw/airborne/modules/computer_vision/cv.c +++ b/sw/airborne/modules/computer_vision/cv.c @@ -43,7 +43,7 @@ static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B) } -struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func) +struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps) { // Create a new video listener struct video_listener *new_listener = malloc(sizeof(struct video_listener)); @@ -53,7 +53,7 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi new_listener->func = func; new_listener->next = NULL; new_listener->async = NULL; - new_listener->maximum_fps = 0; + new_listener->maximum_fps = fps; // Initialise the device that we want our function to use add_video_device(device); @@ -79,10 +79,10 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi } -struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level) +struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps) { // Create a normal listener - struct video_listener *listener = cv_add_to_device(device, func); + struct video_listener *listener = cv_add_to_device(device, func, fps); // Add asynchronous structure to override default synchronous behavior listener->async = malloc(sizeof(struct cv_async)); diff --git a/sw/airborne/modules/computer_vision/cv.h b/sw/airborne/modules/computer_vision/cv.h index a662b9011e..9d0c999fa8 100644 --- a/sw/airborne/modules/computer_vision/cv.h +++ b/sw/airborne/modules/computer_vision/cv.h @@ -61,8 +61,8 @@ struct video_listener { extern bool add_video_device(struct video_config_t *device); -extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func); -extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level); +extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps); +extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps); extern void cv_run_device(struct video_config_t *device, struct image_t *img); diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.c b/sw/airborne/modules/computer_vision/cv_blob_locator.c index 8bd5e8943f..c75a7efdd3 100644 --- a/sw/airborne/modules/computer_vision/cv_blob_locator.c +++ b/sw/airborne/modules/computer_vision/cv_blob_locator.c @@ -53,11 +53,13 @@ volatile bool marker_enabled = false; volatile bool window_enabled = false; // Computer vision thread -struct image_t* cv_marker_func(struct image_t *img); -struct image_t* cv_marker_func(struct image_t *img) { +struct image_t *cv_marker_func(struct image_t *img); +struct image_t *cv_marker_func(struct image_t *img) +{ - if (!marker_enabled) + if (!marker_enabled) { return NULL; + } struct marker_deviation_t m = marker(img, marker_size); @@ -73,14 +75,16 @@ struct image_t* cv_marker_func(struct image_t *img) { // Computer vision thread -struct image_t* cv_window_func(struct image_t *img); -struct image_t* cv_window_func(struct image_t *img) { +struct image_t *cv_window_func(struct image_t *img); +struct image_t *cv_window_func(struct image_t *img) +{ - if (!window_enabled) + if (!window_enabled) { return NULL; + } - uint16_t coordinate[2] = {0,0}; + uint16_t coordinate[2] = {0, 0}; uint16_t response = 0; uint32_t integral_image[img->w * img->h]; @@ -88,7 +92,7 @@ struct image_t* cv_window_func(struct image_t *img) { image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE); image_to_grayscale(img, &gray); - response = detect_window_sizes( (uint8_t*)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT); + response = detect_window_sizes((uint8_t *)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT); image_free(&gray); @@ -98,13 +102,13 @@ struct image_t* cv_window_func(struct image_t *img) { if (response < 92) { - for (int y = 0; y < img->h-1; y++) { + for (int y = 0; y < img->h - 1; y++) { Img(px, y) = 65; - Img(px+1, y) = 255; + Img(px + 1, y) = 255; } - for (int x = 0; x < img->w-1; x+=2) { + for (int x = 0; x < img->w - 1; x += 2) { Img(x, py) = 65; - Img(x+1, py) = 255; + Img(x + 1, py) = 255; } uint32_t temp = coordinate[0]; @@ -118,11 +122,13 @@ struct image_t* cv_window_func(struct image_t *img) { } -struct image_t* cv_blob_locator_func(struct image_t *img); -struct image_t* cv_blob_locator_func(struct image_t *img) { +struct image_t *cv_blob_locator_func(struct image_t *img); +struct image_t *cv_blob_locator_func(struct image_t *img) +{ - if (!blob_enabled) + if (!blob_enabled) { return NULL; + } // Color Filter @@ -137,9 +143,9 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { // Output image struct image_t dst; image_create(&dst, - img->w, - img->h, - IMAGE_GRADIENT); + img->w, + img->h, + IMAGE_GRADIENT); // Labels uint16_t labels_count = 512; @@ -152,7 +158,7 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { int largest_size = 0; // Find largest - for (int i=0; i 50) { if (labels[i].pixel_cnt > largest_size) { @@ -162,21 +168,20 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { } } - if (largest_id >= 0) - { - uint8_t *p = (uint8_t*) img->buf; - uint16_t* l = (uint16_t*) dst.buf; - for (int y=0;y= 0) { + uint8_t *p = (uint8_t *) img->buf; + uint16_t *l = (uint16_t *) dst.buf; + for (int y = 0; y < dst.h; y++) { + for (int x = 0; x < dst.w / 2; x++) { + if (l[y * dst.w + x] != 0xffff) { + uint8_t c = 0xff; + if (l[y * dst.w + x] == largest_id) { c = 0; } - p[y*dst.w*2+x*4]=c; - p[y*dst.w*2+x*4+1]=0x80; - p[y*dst.w*2+x*4+2]=c; - p[y*dst.w*2+x*4+3]=0x80; + p[y * dst.w * 2 + x * 4] = c; + p[y * dst.w * 2 + x * 4 + 1] = 0x80; + p[y * dst.w * 2 + x * 4 + 2] = c; + p[y * dst.w * 2 + x * 4 + 3] = 0x80; } } } @@ -185,19 +190,19 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { uint16_t cgx = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2; uint16_t cgy = labels[largest_id].y_sum / labels[largest_id].pixel_cnt; - if ((cgx > 1) && (cgx < (dst.w-2)) && - (cgy > 1) && (cgy < (dst.h-2)) - ) { - p[cgy*dst.w*2+cgx*2-4] = 0xff; - p[cgy*dst.w*2+cgx*2-2] = 0x00; - p[cgy*dst.w*2+cgx*2] = 0xff; - p[cgy*dst.w*2+cgx*2+2] = 0x00; - p[cgy*dst.w*2+cgx*2+4] = 0xff; - p[cgy*dst.w*2+cgx*2+6] = 0x00; - p[(cgy-1)*dst.w*2+cgx*2] = 0xff; - p[(cgy-1)*dst.w*2+cgx*2+2] = 0x00; - p[(cgy+1)*dst.w*2+cgx*2] = 0xff; - p[(cgy+1)*dst.w*2+cgx*2+2] = 0x00; + if ((cgx > 1) && (cgx < (dst.w - 2)) && + (cgy > 1) && (cgy < (dst.h - 2)) + ) { + p[cgy * dst.w * 2 + cgx * 2 - 4] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 - 2] = 0x00; + p[cgy * dst.w * 2 + cgx * 2] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 + 2] = 0x00; + p[cgy * dst.w * 2 + cgx * 2 + 4] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 + 6] = 0x00; + p[(cgy - 1)*dst.w * 2 + cgx * 2] = 0xff; + p[(cgy - 1)*dst.w * 2 + cgx * 2 + 2] = 0x00; + p[(cgy + 1)*dst.w * 2 + cgx * 2] = 0xff; + p[(cgy + 1)*dst.w * 2 + cgx * 2 + 2] = 0x00; } @@ -217,7 +222,8 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { #include -void cv_blob_locator_init(void) { +void cv_blob_locator_init(void) +{ // Red board in sunlight color_lum_min = 100; color_lum_max = 200; @@ -238,40 +244,41 @@ void cv_blob_locator_init(void) { georeference_init(); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS); } -void cv_blob_locator_periodic(void) { +void cv_blob_locator_periodic(void) +{ } -void cv_blob_locator_event(void) { - switch (cv_blob_locator_type) - { - case 1: - blob_enabled = true; - marker_enabled = false; - window_enabled = false; - break; - case 2: - blob_enabled = false; - marker_enabled = true; - window_enabled = false; - break; - case 3: - blob_enabled = false; - marker_enabled = false; - window_enabled = true; - break; - default: - blob_enabled = false; - marker_enabled = false; - window_enabled = false; - break; +void cv_blob_locator_event(void) +{ + switch (cv_blob_locator_type) { + case 1: + blob_enabled = true; + marker_enabled = false; + window_enabled = false; + break; + case 2: + blob_enabled = false; + marker_enabled = true; + window_enabled = false; + break; + case 3: + blob_enabled = false; + marker_enabled = false; + window_enabled = true; + break; + default: + blob_enabled = false; + marker_enabled = false; + window_enabled = false; + break; } if (blob_locator != 0) { // CV thread has results: import @@ -282,11 +289,11 @@ void cv_blob_locator_event(void) { uint16_t y = temp & 0x0000ffff; temp = temp >> 16; uint16_t x = temp & 0x0000ffff; - printf("Found %d %d \n",x,y); + printf("Found %d %d \n", x, y); struct camera_frame_t cam; - cam.px = x/2; - cam.py = y/2; + cam.px = x / 2; + cam.py = y / 2; cam.f = 400; cam.h = 240; cam.w = 320; @@ -295,31 +302,36 @@ void cv_blob_locator_event(void) { georeference_project(&cam, WP_p1); #endif #ifdef WP_CAM - georeference_filter(FALSE,WP_CAM, geofilter_length); + georeference_filter(FALSE, WP_CAM, geofilter_length); #endif } } -extern void cv_blob_locator_start(void) { +extern void cv_blob_locator_start(void) +{ georeference_init(); } -extern void cv_blob_locator_stop(void) { +extern void cv_blob_locator_stop(void) +{ } -void start_vision(void) { +void start_vision(void) +{ georeference_init(); record_video = 1; cv_blob_locator_type = 3; } -void start_vision_land(void) { +void start_vision_land(void) +{ georeference_init(); record_video = 1; cv_blob_locator_type = 2; } -void stop_vision(void) { +void stop_vision(void) +{ georeference_init(); record_video = 0; cv_blob_locator_type = 0; diff --git a/sw/airborne/modules/computer_vision/cv_opencvdemo.c b/sw/airborne/modules/computer_vision/cv_opencvdemo.c index cbb022120e..aaa145869f 100644 --- a/sw/airborne/modules/computer_vision/cv_opencvdemo.c +++ b/sw/airborne/modules/computer_vision/cv_opencvdemo.c @@ -29,14 +29,13 @@ // Function -struct image_t* opencv_func(struct image_t* img); -struct image_t* opencv_func(struct image_t* img) +struct image_t *opencv_func(struct image_t *img); +struct image_t *opencv_func(struct image_t *img) { - if (img->type == IMAGE_YUV422) - { + if (img->type == IMAGE_YUV422) { // Call OpenCV (C++ from paparazzi C function) - opencv_example((char*) img->buf, img->w, img->h); + opencv_example((char *) img->buf, img->w, img->h); } // opencv_example(NULL, 10,10); @@ -46,6 +45,6 @@ struct image_t* opencv_func(struct image_t* img) void opencvdemo_init(void) { - cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func); + cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS); } diff --git a/sw/airborne/modules/computer_vision/detect_contour.c b/sw/airborne/modules/computer_vision/detect_contour.c index 4d3db0dc17..eda233b709 100644 --- a/sw/airborne/modules/computer_vision/detect_contour.c +++ b/sw/airborne/modules/computer_vision/detect_contour.c @@ -41,7 +41,7 @@ struct image_t *contour_func(struct image_t *img) void detect_contour_init(void) { - cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func); + cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS); // in the mavlab, bright cont_thres.lower_y = 16; cont_thres.lower_u = 135; cont_thres.lower_v = 80; cont_thres.upper_y = 100; cont_thres.upper_u = 175; cont_thres.upper_v = 165; diff --git a/sw/airborne/modules/computer_vision/detect_window.c b/sw/airborne/modules/computer_vision/detect_window.c index c0cf08b3a5..d5fe816773 100644 --- a/sw/airborne/modules/computer_vision/detect_window.c +++ b/sw/airborne/modules/computer_vision/detect_window.c @@ -33,10 +33,10 @@ void detect_window_init(void) { - cv_add_to_device(&BLOB_LOCATOR_CAMERA, detect_window); + cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS); } -struct image_t* detect_window(struct image_t *img) +struct image_t *detect_window(struct image_t *img) { uint16_t coordinate[2]; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 90a4e46c72..7809e11724 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -126,7 +126,7 @@ void opticflow_module_init(void) opticflow_got_result = false; opticflow_calc_init(&opticflow); - cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc); + cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send); diff --git a/sw/airborne/modules/computer_vision/qrcode/qr_code.c b/sw/airborne/modules/computer_vision/qrcode/qr_code.c index e9bbcd8c56..fba4e5357a 100644 --- a/sw/airborne/modules/computer_vision/qrcode/qr_code.c +++ b/sw/airborne/modules/computer_vision/qrcode/qr_code.c @@ -37,7 +37,7 @@ bool drawRectangleAroundQRCode = QRCODE_DRAW_RECTANGLE; void qrcode_init(void) { // Add qrscan to the list of image processing tasks in video_thread - cv_add_to_device(&QRCODE_CAMERA, qrscan); + cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS); } // Telemetry diff --git a/sw/airborne/modules/computer_vision/video_capture.c b/sw/airborne/modules/computer_vision/video_capture.c index aa20d15633..f6a6d35d61 100644 --- a/sw/airborne/modules/computer_vision/video_capture.c +++ b/sw/airborne/modules/computer_vision/video_capture.c @@ -66,7 +66,7 @@ void video_capture_init(void) } // Add function to computer vision pipeline - cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func); + cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS); } diff --git a/sw/airborne/modules/computer_vision/video_usb_logger.c b/sw/airborne/modules/computer_vision/video_usb_logger.c index 3296e96be8..d1fa4c0467 100644 --- a/sw/airborne/modules/computer_vision/video_usb_logger.c +++ b/sw/airborne/modules/computer_vision/video_usb_logger.c @@ -143,7 +143,7 @@ void video_usb_logger_start(void) } // Subscribe to a camera - cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image); + cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS); } /** Stop the logger an nicely close the file */ diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 2a233c58e5..8e9f294398 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -246,15 +246,13 @@ void viewvideo_init(void) #ifdef VIEWVIDEO_CAMERA struct video_listener *listener1 = cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1, - VIEWVIDEO_NICE_LEVEL); - listener1->maximum_fps = VIEWVIDEO_FPS; - fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA1\n"); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA1 at %u FPS \n", VIEWVIDEO_FPS); #endif #ifdef VIEWVIDEO_CAMERA2 struct video_listener *listener2 = cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2, - VIEWVIDEO_NICE_LEVEL); - listener2->maximum_fps = VIEWVIDEO_FPS; - fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA2\n"); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA2 at %u FPS \n", VIEWVIDEO_FPS); #endif }