void cv_blob_locator_init(void) {
// Red board in sunlight
color_lum_min = 100;
color_lum_max = 200;
color_cb_min = 140;
color_cb_max = 255;
color_cr_min = 140;
color_cr_max = 255;
// Lamp during night
color_lum_min = 180;
color_lum_max = 255;
color_cb_min = 100;
color_cb_max = 150;
color_cr_min = 100;
color_cr_max = 150;
cv_blob_locator_reset = 0;
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);
}
/** Start the file logger and open a new file */
void video_usb_logger_start(void)
{
uint32_t counter = 0;
char filename[512];
struct stat st = {0};
// Search and create a new folder
do {
snprintf(foldername, sizeof(foldername), "%s/pprzvideo%05d", STRINGIFY(VIDEO_USB_LOGGER_PATH), counter);
counter++;
} while (stat(foldername, &st) >= 0);
mkdir(foldername, 0700);
// In this folder create a textlog
snprintf(filename, sizeof(filename), "%s/log.csv", foldername);
video_usb_logger = fopen(filename, "w");
if (video_usb_logger != NULL) {
fprintf(video_usb_logger, "counter,image,roll,pitch,yaw,x,y,z,accelx,accely,accelz,ratep,rateq,rater,sonar\n");
}
// Subscribe to a camera
cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image);
}
/** Start the file logger and open a new file */
void video_usb_logger_start(void)
{
// Create the jpeg image used later
image_create(&img_jpeg, VIDEO_USB_LOGGER_WIDTH, VIDEO_USB_LOGGER_HEIGHT, IMAGE_JPEG);
uint32_t counter = 0;
char filename[512];
// Search and create a new folder
sprintf(foldername, "%s/pprzvideo%05d", STRINGIFY(VIDEO_USB_LOGGER_PATH), counter);
struct stat st = {0};
while (stat(foldername, &st) >= 0) {
counter++;
sprintf(foldername, "%s/pprzvideo%05d", STRINGIFY(VIDEO_USB_LOGGER_PATH), counter);
}
mkdir(foldername, 0700);
// In this folder create a textlog
sprintf(filename, "%s/log.csv", foldername);
video_usb_logger = fopen(filename, "w");
if (video_usb_logger != NULL) {
fprintf(video_usb_logger, "counter,image,roll,pitch,yaw,x,y,z,sonar\n");
}
// Subscribe to a camera
cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image);
}
void detect_gate_init(void)
{
// settings:
just_filtering = DETECT_GATE_JUST_FILTER;
n_samples = DETECT_GATE_N_SAMPLES;
min_px_size = DETECT_GATE_MIN_PIX_SIZE;
min_gate_quality = DETECT_GATE_MIN_GATE_QUALITY;
min_n_sides = DETECT_GATE_MIN_N_SIDES;
gate_thickness = DETECT_GATE_GATE_THICKNESS;
color_Ym = DETECT_GATE_Y_MIN;
color_YM = DETECT_GATE_Y_MAX;
color_Um = DETECT_GATE_U_MIN;
color_UM = DETECT_GATE_U_MAX;
color_Vm = DETECT_GATE_V_MIN;
color_VM = DETECT_GATE_V_MAX;
exclude_top = DETECT_GATE_EXCLUDE_PIXELS_TOP;
exclude_bottom = DETECT_GATE_EXCLUDE_PIXELS_BOTTOM;
// World coordinates: X positive towards the gate, Z positive down, Y positive right:
#if !CAMERA_ROTATED_90DEG_RIGHT
// Top-left, CW:
VECT3_ASSIGN(world_corners[0],
0.0f, -(gate_size_m / 2), gate_center_height - (gate_size_m / 2));
VECT3_ASSIGN(world_corners[1],
0.0f, (gate_size_m / 2), gate_center_height - (gate_size_m / 2));
VECT3_ASSIGN(world_corners[2],
0.0f, (gate_size_m / 2), gate_center_height + (gate_size_m / 2));
VECT3_ASSIGN(world_corners[3],
0.0f, -(gate_size_m / 2), gate_center_height + (gate_size_m / 2));
#else
// Bottom-right, CCW:
VECT3_ASSIGN(world_corners[0],
0.0f, (gate_size_m / 2), gate_center_height + (gate_size_m / 2));
VECT3_ASSIGN(world_corners[1],
0.0f, (gate_size_m / 2), gate_center_height - (gate_size_m / 2));
VECT3_ASSIGN(world_corners[2],
0.0f, -(gate_size_m / 2), gate_center_height - (gate_size_m / 2));
VECT3_ASSIGN(world_corners[3],
0.0f, -(gate_size_m / 2), gate_center_height + (gate_size_m / 2));
#endif
cam_body.phi = 0;
cam_body.theta = 0;
cam_body.psi = 0;
// Shared variables to copy data from thread to module
pthread_mutex_init(&gate_detect_mutex, NULL);
detect_gate_has_new_data = false;
detect_gate_x = 0;
detect_gate_y = 0;
detect_gate_z = 0;
cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VISION_POSITION_ESTIMATE, send_detect_gate_visual_position);
}
void detect_contour_init(void)
{
cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func);
// 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;
//
// for cyberzoo: Y:12-95, U:129-161, V:80-165, turn white.
//int y1=16; int u1=129; int v1=80; % cyberzoo, dark
//int y2=100; int u2=161; int v2=165;
}
void video_capture_init(void)
{
// Create the images directory
char save_name[128];
sprintf(save_name, "mkdir -p %s", STRINGIFY(VIDEO_CAPTURE_PATH));
if (system(save_name) != 0) {
printf("[video_capture] Could not create images directory %s.\n", STRINGIFY(VIDEO_CAPTURE_PATH));
return;
}
// Add function to computer vision pipeline
cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func);
}
/**
* Initialize the optical flow module for the bottom camera
*/
void opticflow_module_init(void)
{
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb);
// Set the opticflow state to 0
FLOAT_RATES_ZERO(opticflow_state.rates);
opticflow_state.agl = 0;
// Initialize the opticflow calculation
opticflow_got_result = false;
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
#endif
}
/**
* Initialize the view video
*/
void viewvideo_init(void)
{
char save_name[512];
cv_add_to_device(&VIEWVIDEO_CAMERA, viewvideo_function);
viewvideo.is_streaming = true;
#if VIEWVIDEO_USE_NETCAT
// Create an Netcat receiver file for the streaming
sprintf(save_name, "%s/netcat-recv.sh", STRINGIFY(VIEWVIDEO_SHOT_PATH));
FILE *fp = fopen(save_name, "w");
if (fp != NULL) {
fprintf(fp, "i=0\n");
fprintf(fp, "while true\n");
fprintf(fp, "do\n");
fprintf(fp, "\tn=$(printf \"%%04d\" $i)\n");
fprintf(fp, "\tnc -l 0.0.0.0 %d > img_${n}.jpg\n", (int)(VIEWVIDEO_PORT_OUT));
fprintf(fp, "\ti=$((i+1))\n");
fprintf(fp, "done\n");
fclose(fp);
} else {
printf("[viewvideo] Failed to create netcat receiver file.\n");
}
#else
// Open udp socket
udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
// Create an SDP file for the streaming
sprintf(save_name, "%s/stream.sdp", STRINGIFY(VIEWVIDEO_SHOT_PATH));
FILE *fp = fopen(save_name, "w");
if (fp != NULL) {
fprintf(fp, "v=0\n");
fprintf(fp, "m=video %d RTP/AVP 26\n", (int)(VIEWVIDEO_PORT_OUT));
fprintf(fp, "c=IN IP4 0.0.0.0\n");
fclose(fp);
} else {
printf("[viewvideo] Failed to create SDP file.\n");
}
#endif
}
void detect_window_init(void)
{
cv_add_to_device(&BLOB_LOCATOR_CAMERA, detect_window);
}
void opencvdemo_init(void)
{
cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func);
}
void video_crop_init(void)
{
// cv_add(video_crop_func);
cv_add_to_device(&CAMERA,video_crop_func);
}