diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml index a0871daf3b..d6f93006ec 100644 --- a/conf/airframes/ENAC/conf_enac.xml +++ b/conf/airframes/ENAC/conf_enac.xml @@ -60,7 +60,7 @@ airframe="airframes/ENAC/quadrotor/crazyflie_2.1.xml" radio="radios/cockpitSX.xml" telemetry="telemetry/default_rotorcraft.xml" - flight_plan="flight_plans/rotorcraft_basic.xml" + flight_plan="flight_plans/ENAC/crazyflie_multi_ranger_test.xml" settings="settings/rotorcraft_basic.xml" settings_modules="modules/ahrs_madgwick.xml modules/air_data.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/opticflow_pmw3901.xml modules/range_forcefield.xml modules/sonar_vl53l1x.xml modules/stabilization_int_quat.xml" gui_color="blue" diff --git a/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml b/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml new file mode 100644 index 0000000000..33d85b41be --- /dev/null +++ b/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml @@ -0,0 +1,118 @@ + + + +
+#include "autopilot_guided.h" + +// Useful Combination of the flags fir the autopilot_guided_update +#define GUIDED_FLAG_XY_VEL_BODY GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL +#define GUIDED_FLAG_XY_VEL_BODY_YAW_RATE GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_RATE +#define GUIDED_FLAG_XY_OFFSET_Z_VEL GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_Z_VEL +#define GUIDED_FLAG_XY_OFFSET_YAW_OFFSET GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_YAW_OFFSET + +#ifdef NAV_C +static float obstacle_dist, obstacle_azimuth, obstacle_bearing; +static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused)), float _obstacle_dist, float _obstacle_azimuth, float _obstacle_bearing) +{ + if (And(CloseRadAngles(_obstacle_azimuth,0), CloseRadAngles(_obstacle_bearing,0))) { + obstacle_dist = _obstacle_dist; + obstacle_azimuth = _obstacle_azimuth; + obstacle_bearing = _obstacle_bearing; + } +} +#endif +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 4743da66af..69c77585ec 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -338,7 +338,7 @@ radio="radios/spektrum.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/control/stabilization_att_int.xml settings/nps.xml settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml" + settings="settings/nps.xml settings/rotorcraft_basic.xml" settings_modules="modules/sys_id_chirp.xml modules/air_data.xml modules/geo_mag.xml modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="blue" release="f739a81cfa2c633e33f31ab5f095d8f617276974" @@ -507,7 +507,7 @@ radio="radios/dummy.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_optitrack.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml" + settings="settings/rotorcraft_basic.xml" settings_modules="modules/optical_flow_hover.xml modules/cv_opticflow.xml modules/bebop_cam.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="red" /> diff --git a/conf/userconf/tudelft/mk_conf.xml b/conf/userconf/tudelft/mk_conf.xml index 902fee56b2..5e729277f8 100644 --- a/conf/userconf/tudelft/mk_conf.xml +++ b/conf/userconf/tudelft/mk_conf.xml @@ -6,7 +6,7 @@ radio="radios/delfly_Rx31_DEVO10.xml" telemetry="telemetry/rotorcraft_with_logger.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml ../conf/modules/logger_sd_spi_direct.xml" + settings="settings/rotorcraft_basic.xml" settings_modules="modules/logger_sd_spi_direct.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="blue" /> @@ -17,7 +17,7 @@ radio="radios/delfly_nimble_Rx31_Devo10.xml" telemetry="telemetry/rotorcraft_with_logger.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml" + settings="settings/rotorcraft_basic.xml" settings_modules="modules/logger_sd_spi_direct.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="blue" /> diff --git a/sw/airborne/modules/computer_vision/cv_detect_color_object.c b/sw/airborne/modules/computer_vision/cv_detect_color_object.c index b0b4ccd042..fe84092d4b 100644 --- a/sw/airborne/modules/computer_vision/cv_detect_color_object.c +++ b/sw/airborne/modules/computer_vision/cv_detect_color_object.c @@ -140,13 +140,13 @@ static struct image_t *object_detector(struct image_t *img, uint8_t filter) } struct image_t *object_detector1(struct image_t *img, uint8_t camera_id); -struct image_t *object_detector1(struct image_t *img, uint8_t camera_id) +struct image_t *object_detector1(struct image_t *img, uint8_t camera_id __attribute__((unused))) { return object_detector(img, 1); } struct image_t *object_detector2(struct image_t *img, uint8_t camera_id); -struct image_t *object_detector2(struct image_t *img, uint8_t camera_id) +struct image_t *object_detector2(struct image_t *img, uint8_t camera_id __attribute__((unused))) { return object_detector(img, 2); } diff --git a/sw/airborne/modules/computer_vision/undistort_image.c b/sw/airborne/modules/computer_vision/undistort_image.c index e8f71450fd..eafc3ec04c 100644 --- a/sw/airborne/modules/computer_vision/undistort_image.c +++ b/sw/airborne/modules/computer_vision/undistort_image.c @@ -118,5 +118,5 @@ void undistort_image_init(void) min_x_normalized = UNDISTORT_MIN_X_NORMALIZED; max_x_normalized = UNDISTORT_MAX_X_NORMALIZED; center_ratio = UNDISTORT_CENTER_RATIO; - listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS), 0; + listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS, 0); } diff --git a/sw/airborne/modules/ctrl/optical_flow_hover.c b/sw/airborne/modules/ctrl/optical_flow_hover.c index 8534d608a8..5f640f3ed4 100644 --- a/sw/airborne/modules/ctrl/optical_flow_hover.c +++ b/sw/airborne/modules/ctrl/optical_flow_hover.c @@ -185,8 +185,8 @@ float vision_time, prev_vision_timeXY, prev_vision_timeZ; bool oscillatingX; bool oscillatingY; -int16_t flowX; -int16_t flowY; +int32_t flowX; +int32_t flowY; struct OFhistory historyX; struct OFhistory historyY; @@ -598,8 +598,8 @@ void vertical_ctrl_module_run(bool in_flight) stabilization_cmd[COMMAND_THRUST] = des_inputs.thrust; } -void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int16_t flow_x, int16_t flow_y, - int16_t flow_der_x, int16_t flow_der_y, float quality, float size_div) +void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int32_t flow_x, int32_t flow_y, + int32_t flow_der_x, int32_t flow_der_y, float quality, float size_div) { if (!derotated) { flowX = flow_x; diff --git a/sw/airborne/modules/wedgebug/wedgebug.c b/sw/airborne/modules/wedgebug/wedgebug.c index 25725476b9..1e50f7fc3e 100644 --- a/sw/airborne/modules/wedgebug/wedgebug.c +++ b/sw/airborne/modules/wedgebug/wedgebug.c @@ -289,14 +289,6 @@ void show_image_entry(struct image_t *img, int entry_position, const char *img_name); // Function 3: Displays pixel value of image void show_rotation_matrix(struct FloatRMat *R); -// External -void post_disparity_crop_rect(struct crop_t *img_cropped_info, struct img_size_t *original_img_dims, const int disp_n, - const int block_size); -void set_state(uint8_t state, uint8_t change_allowed); -void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type); -void kernel_free(struct kernel_C1 *kernel); -uint8_t getMedian(uint8_t *a, uint32_t n); - //Core static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id); // Function 1: Copies left image into a buffer (buf_left) @@ -406,7 +398,7 @@ void show_rotation_matrix(struct FloatRMat *R) // Function 1 - Returns the upper left coordinates of a square (x and y coordinates) and the offset in terms of width and height, // given the number of disparity levels and the block size used by the block matching algorithm. This is need to crop an image -void post_disparity_crop_rect(struct crop_t *img_cropped_info, struct img_size_t *original_img_dims, const int disp_n, +void post_disparity_crop_rect(struct crop_t *_img_cropped_info, struct img_size_t *_original_img_dims, const int disp_n, const int block_size) { @@ -414,20 +406,20 @@ void post_disparity_crop_rect(struct crop_t *img_cropped_info, struct img_size_t uint16_t left_black = disp_n + block_size_black; - img_cropped_info->y = block_size_black; - img_cropped_info->h = original_img_dims->h - block_size_black; - img_cropped_info->h = img_cropped_info->h - img_cropped_info->y; + _img_cropped_info->y = block_size_black; + _img_cropped_info->h = _original_img_dims->h - block_size_black; + _img_cropped_info->h = _img_cropped_info->h - _img_cropped_info->y; - img_cropped_info->x = left_black - 1; - img_cropped_info->w = original_img_dims->w - block_size_black; - img_cropped_info->w = img_cropped_info->w - img_cropped_info->x; + _img_cropped_info->x = left_black - 1; + _img_cropped_info->w = _original_img_dims->w - block_size_black; + _img_cropped_info->w = _img_cropped_info->w - _img_cropped_info->x; } // Function 2 - Sets finite state machine state (useful for the flight path blocks) -void set_state(uint8_t state, uint8_t change_allowed) +void set_state(uint8_t _state, uint8_t change_allowed) { - if (change_allowed == 1) {current_state = state;} + if (change_allowed == 1) {current_state = _state;} } @@ -540,7 +532,7 @@ uint16_t getMedian16bit(uint16_t *a, uint32_t n) // Core: // Function 1 -static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id) +static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id __attribute__((unused))) { image_copy(img, &img_left); //show_image_data(img); @@ -550,7 +542,7 @@ static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id // Function 2 -static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id) +static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id __attribute__((unused))) { image_copy(img, &img_right); //show_image_data(img); @@ -688,38 +680,38 @@ void thresholding_img(struct image_t *img, uint8_t threshold) // Function 7 - Calculates principal point coordinates for a cropped image, based on the x // and y coordinates of the cropped area (upper left-hand side: crop_y and crop_x). -void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info) +void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *_img_cropped_info) { - c_output->y = c_old_input->y - img_cropped_info->y; - c_output->x = c_old_input->x - img_cropped_info->x; + c_output->y = c_old_input->y - _img_cropped_info->y; + c_output->x = c_old_input->x - _img_cropped_info->x; } // Function 8a - Converts disparity to depth using focal length (in pixels) and baseline distance (in meters) -float disp_to_depth(const uint8_t d, const float b, const uint16_t f) +float disp_to_depth(const uint8_t _d, const float _b, const uint16_t _f) { - return b * f / d; + return _b * _f / _d; } // Function 8c - Converts 16bit fixed number disparity (pixels * 16) to 16bit disparity (pixels) -float dispfixed_to_disp(const int16_t d) +float dispfixed_to_disp(const int16_t _d) { - return (d / 16.00); + return (_d / 16.00); } // Function 8d - Converts 16bit fixed number disparity (pixels * 16) to 16bit depth (meters) using focal length (in pixels) and baseline distance (in meters) -float disp_to_depth_16bit(const int16_t d, const float b, const uint16_t f) +float disp_to_depth_16bit(const int16_t _d, const float _b, const uint16_t _f) { - return b * f / dispfixed_to_disp(d); + return _b * _f / dispfixed_to_disp(_d); } // Function 8b - Converts depth to disparity using focal length (in pixels) and baseline distance (in meters) -uint8_t depth_to_disp(const float depth, const float b, const uint16_t f) +uint8_t depth_to_disp(const float _depth, const float _b, const uint16_t _f) { - return round(b * f / depth); + return round(_b * _f / _depth); } @@ -727,8 +719,8 @@ uint8_t depth_to_disp(const float depth, const float b, const uint16_t f) // Function 9a - Calculates 3d points in a scene based on the 2d coordinates of the point in the // image plane and the depth. d in in pixels, b is in meters and f is in pixels -void Vi_to_Vc(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint8_t d, - const float b, const uint16_t f) +void Vi_to_Vc(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint8_t _d, + const float _b, const uint16_t _f) { // Calculating Z // In case disparity is 0 Z will be very very small to avoid detection of algorithm that @@ -738,20 +730,20 @@ void Vi_to_Vc(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t ima //printf("d=%d\n", d); - if (d == 0) { + if (_d == 0) { scene_point->z = 0.0001; } else { - scene_point->z = disp_to_depth(d, b, f); + scene_point->z = disp_to_depth(_d, _b, _f); } //printf("Z=%f\n", scene_point->Z); // Calculating Y - scene_point->y = image_point_y * scene_point -> z / f; + scene_point->y = image_point_y * scene_point -> z / _f; // Calculating X - scene_point->x = image_point_x * scene_point -> z / f; + scene_point->x = image_point_x * scene_point -> z / _f; //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y); //printf("X (x=%d) = %f\n", image_point->x, scene_point->X); //printf("Z (d=%d) = %f\n", d, scene_point->Z); @@ -761,16 +753,16 @@ void Vi_to_Vc(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t ima // Function 9b - Calculates 3d points in a scene based on the 2d coordinates of the point in the // image plane and the depth. d in in pixels, b is in meters and f is in pixels void Vi_to_Vc_depth(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, - const float depth /*depth in m*/, const uint16_t f) + const float depth /*depth in m*/, const uint16_t _f) { // Calculating Z scene_point->z = depth; // Calculating Y - scene_point->y = image_point_y * scene_point -> z / f; + scene_point->y = image_point_y * scene_point -> z / _f; // Calculating X - scene_point->x = image_point_x * scene_point -> z / f; + scene_point->x = image_point_x * scene_point -> z / _f; //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y); //printf("X (x=%d) = %f\n", image_point->x, scene_point->X); //printf("Z (d=%d) = %f\n", d, scene_point->Z); @@ -779,8 +771,8 @@ void Vi_to_Vc_depth(struct FloatVect3 *scene_point, int32_t image_point_y, int32 // Function 9c - Calculates 3d points in a scene based on the 2d coordinates of the point in the // image plane and the depth. d in in pixels, b is in meters and f is in pixels -void Vi_to_Vc16bit(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint16_t d, - const float b, const uint16_t f) +void Vi_to_Vc16bit(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint16_t _d, + const float _b, const uint16_t _f) { // Calculating Z // In case disparity is 0 Z will be very very small to avoid detection of algorithm that @@ -790,20 +782,20 @@ void Vi_to_Vc16bit(struct FloatVect3 *scene_point, int32_t image_point_y, int32_ //printf("d=%d\n", d); - if (d == 0) { + if (_d == 0) { scene_point->z = 0.0001; } else { - scene_point->z = disp_to_depth_16bit(d, b, f); + scene_point->z = disp_to_depth_16bit(_d, _b, _f); } //printf("Z=%f\n", scene_point->Z); // Calculating Y - scene_point->y = image_point_y * scene_point -> z / f; + scene_point->y = image_point_y * scene_point -> z / _f; // Calculating X - scene_point->x = image_point_x * scene_point -> z / f; + scene_point->x = image_point_x * scene_point -> z / _f; //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y); //printf("X (x=%d) = %f\n", image_point->x, scene_point->X); //printf("Z (d=%d) = %f\n", d, scene_point->Z); @@ -827,16 +819,16 @@ int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img) // Function 10b - Converts 2d coordinates into 1d coordinates (for 1d arrays) - using struct img_size_t for dimensions -int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *img_dims) +int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *_img_dims) { - if (x >= (img_dims->w) || x < 0) { - printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", x, img_dims->w); + if (x >= (_img_dims->w) || x < 0) { + printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", x, _img_dims->w); return -1; - } else if (y >= (img_dims->h) || y < 0) { - printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", y, img_dims->h); + } else if (y >= (_img_dims->h) || y < 0) { + printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", y, _img_dims->h); return -1; } else { - return x + img_dims->w * y; + return x + _img_dims->w * y; } } @@ -892,7 +884,7 @@ void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rb // Function 13 - Function wrapper to convert a point in the world coordinate system to a point in the camera coordinate system void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rrw, struct FloatVect3 *VRw, - struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose) + struct FloatRMat *_Rcr, struct FloatVect3 *_VCr, const uint8_t verbose) { struct FloatVect3 Vr; @@ -925,17 +917,17 @@ void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rr // Rotation matrix - Robot coordinate system expressed in the camera coordinate system printf("Rcr\n"); - printf("%f, %f, %f,\n", Rcr->m[0], Rcr->m[1], Rcr->m[2]); - printf("%f, %f, %f,\n", Rcr->m[3], Rcr->m[4], Rcr->m[5]); - printf("%f, %f, %f\n\n", Rcr->m[6], Rcr->m[7], Rcr->m[8]); + printf("%f, %f, %f,\n", _Rcr->m[0], _Rcr->m[1], _Rcr->m[2]); + printf("%f, %f, %f,\n", _Rcr->m[3], _Rcr->m[4], _Rcr->m[5]); + printf("%f, %f, %f\n\n", _Rcr->m[6], _Rcr->m[7], _Rcr->m[8]); // Vector coordinates - Camera in the robot frame system printf("VCa (camera location)\n"); - printf(" %f\n %f\n %f\n\n", VCr->x, VCr->y, VCr->z); + printf(" %f\n %f\n %f\n\n", _VCr->x, _VCr->y, _VCr->z); } // Camera coordinates from robot coordinates - Va_to_Vb(Vc, &Vr, Rcr, VCr); + Va_to_Vb(Vc, &Vr, _Rcr, _VCr); // Print log only if enabled if (verbose != 0) { @@ -948,12 +940,12 @@ void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rr // Function 14 - Function wrapper to convert a point in the camera coordinate system back to a point in the world coordinate system void Vc_to_Vw(struct FloatVect3 *Vw, struct FloatVect3 *Vc, struct FloatRMat *Rrw, struct FloatVect3 *VRw, - struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose) + struct FloatRMat *_Rcr, struct FloatVect3 *_VCr, const uint8_t verbose) { struct FloatVect3 Vr; // Agent coordinates from camera coordinates - Vb_to_Va(&Vr, Vc, Rcr, VCr); + Vb_to_Va(&Vr, Vc, _Rcr, _VCr); // Print log only if enabled if (verbose != 0) { @@ -1126,11 +1118,11 @@ uint16_t median_depth_to_point(struct point_t *Vi, struct image_t *img, struct k // Returns a 3d Vector to the best "edge" and 1 if any edge is found and 0 if no edge is found. uint8_t find_best_edge_coordinates( struct FloatVect3 - *VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system + *_VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system struct FloatVect3 *VTARGETc, // Declared vector of coordinates of goal in camera coordinate system struct image_t *img_edges, // Image obtained from the external Sobel edge detection function = sobel_OCV struct image_t *img_disparity, // Image obtained after simple block matching - struct crop_t *edge_search_area, // This structure holds information about the window in which edges are searched in + struct crop_t *_edge_search_area, // This structure holds information about the window in which edges are searched in uint8_t threshold, // Above this disparity (pixels) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away) int16_t max_confidence // This is the max confidence that edges were found - Works like a threshold ) @@ -1159,9 +1151,9 @@ uint8_t find_best_edge_coordinates( // Loop to calculate position (in image) of point closes to hypothetical target - Start - for (uint16_t y = edge_search_area->y; y < (edge_search_area->y + edge_search_area->h); - y++) { //for (uint32_t y = edge_search_area.y; y < (edge_search_area.y + edge_search_area.h); y++) - for (uint16_t x = edge_search_area->x; x < (edge_search_area->x + edge_search_area->w); x++) { + for (uint16_t y = _edge_search_area->y; y < (_edge_search_area->y + _edge_search_area->h); + y++) { //for (uint32_t y = _edge_search_area.y; y < (_edge_search_area.y + _edge_search_area.h); y++) + for (uint16_t x = _edge_search_area->x; x < (_edge_search_area->x + _edge_search_area->w); x++) { indx = indx1d_a(y, x, img_edges); // We convert the 2d index [x,y] into a 1d index edge_value = ((uint8_t *) img_edges->buf)[indx]; // We save the intensity of the current point disparity = ((uint8_t *) img_disparity->buf)[indx]; // We save the disparity of the current point @@ -1190,9 +1182,9 @@ uint8_t find_best_edge_coordinates( // measure then save new distance and point coordinates associated with it if ((f_distance_robot_to_edge + f_distance_edge_to_goal) < distance) { // Saving closest edge point, in camera coordinate system - VEDGECOORDINATESc->x = VEDGEc.x; - VEDGECOORDINATESc->y = VEDGEc.y; - VEDGECOORDINATESc->z = VEDGEc.z; + _VEDGECOORDINATESc->x = VEDGEc.x; + _VEDGECOORDINATESc->y = VEDGEc.y; + _VEDGECOORDINATESc->z = VEDGEc.z; // Saving disparity at point disparity_best = disparity; // Saving smallest distance @@ -1240,11 +1232,11 @@ uint8_t find_best_edge_coordinates( // Returns a 3d Vector to the best "edge" and 1 if any edge is found and 0 if no edge is found. uint8_t find_best_edge_coordinates2( struct FloatVect3 - *VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system + *_VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system struct FloatVect3 *VTARGETc, // Declared vector of coordinates of goal in camera coordinate system struct image_t *img_edges, // Image obtained from the external Sobel edge detection function = sobel_OCV struct image_t *img_depth, // Image holding depth values (cm) obtained from the disparity image - struct crop_t *edge_search_area, // This structure holds information about the window in which edges are searched in + struct crop_t *_edge_search_area, // This structure holds information about the window in which edges are searched in uint16_t threshold, // Below this depth (cm) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away) int16_t max_confidence // This is the max confidence that edges were found - Works like a threshold ) @@ -1273,9 +1265,9 @@ uint8_t find_best_edge_coordinates2( // Loop to calculate position (in image) of point closes to hypothetical target - Start - for (uint16_t y = edge_search_area->y; y < (edge_search_area->y + edge_search_area->h); - y++) { //for (uint32_t y = edge_search_area.y; y < (edge_search_area.y + edge_search_area.h); y++) - for (uint16_t x = edge_search_area->x; x < (edge_search_area->x + edge_search_area->w); x++) { + for (uint16_t y = _edge_search_area->y; y < (_edge_search_area->y + _edge_search_area->h); + y++) { //for (uint32_t y = _edge_search_area.y; y < (_edge_search_area.y + _edge_search_area.h); y++) + for (uint16_t x = _edge_search_area->x; x < (_edge_search_area->x + _edge_search_area->w); x++) { indx = indx1d_a(y, x, img_edges); // We convert the 2d index [x,y] into a 1d index edge_value = ((uint8_t *) img_edges->buf)[indx]; // We save the intensity of the current point depth = ((uint16_t *) img_depth->buf)[indx]; @@ -1306,9 +1298,9 @@ uint8_t find_best_edge_coordinates2( // measure then save new distance and point coordinates associated with it if ((f_distance_robot_to_edge + f_distance_edge_to_goal) < distance) { // Saving closest edge point, in camera coordinate system - VEDGECOORDINATESc->x = VEDGEc.x; - VEDGECOORDINATESc->y = VEDGEc.y; - VEDGECOORDINATESc->z = VEDGEc.z; + _VEDGECOORDINATESc->x = VEDGEc.x; + _VEDGECOORDINATESc->y = VEDGEc.y; + _VEDGECOORDINATESc->z = VEDGEc.z; // Saving disparity at point depth_best = depth; // Saving smallest distance @@ -1432,7 +1424,7 @@ void disp_to_depth_img(struct image_t *img_input, struct image_t *img16bit_outpu // Function 24 - Function that encapsulates all of the background processes. Originally this was in the periodic function, // but since it is used in the direct and the guidance navigation modes of the state machine, I made it a callable function // for convenience -void background_processes(uint8_t save_images_flag) +void background_processes(uint8_t _save_images_flag) { //Background processes // 1. Converting left and right image to 8bit grayscale for further processing @@ -1443,7 +1435,7 @@ void background_processes(uint8_t save_images_flag) SBM_OCV(&img_disparity_int8_cropped, &img_left_int8, &img_right_int8, N_disparities, block_size_disparities, 1);// Creating cropped disparity map image // For report: creating image for saving 1 - if (save_images_flag) {save_image_HM(&img_disparity_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img1_post_SBM.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_disparity_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img1_post_SBM.bmp", heat_map_type);} /* // Optional thresholding of disparity map @@ -1461,32 +1453,32 @@ void background_processes(uint8_t save_images_flag) // Needed to smoove object boundaries and to remove noise removing noise opening_OCV(&img_disparity_int8_cropped, &img_middle_int8_cropped, SE_opening_OCV, 1); // For report: creating image for saving 2 - if (save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img2_post_opening_8bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img2_post_opening_8bit.bmp", heat_map_type);} closing_OCV(&img_middle_int8_cropped, &img_middle_int8_cropped, SE_closing_OCV, 1); // For report: creating image for saving 3 - if (save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img3_post_closing_8bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img3_post_closing_8bit.bmp", heat_map_type);} dilation_OCV(&img_middle_int8_cropped, &img_middle_int8_cropped, SE_dilation_OCV_1, 1); // For report: creating image for saving 4 - if (save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img4_post_dilation_8bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img4_post_dilation_8bit.bmp", heat_map_type);} // 4. Depth image disp_to_depth_img(&img_middle_int8_cropped, &img_depth_int16_cropped); // For report: creating image for saving 4 - if (save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img5_post_depth_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img5_post_depth_16bit.bmp", heat_map_type);} // 5. Sobel edge detection sobel_OCV(&img_depth_int16_cropped, &img_edges_int8_cropped, SE_sobel_OCV, threshold_edge_magnitude); // For report: creating image for saving 5 - if (save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img6_post_sobel_8bit.bmp");} + if (_save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img6_post_sobel_8bit.bmp");} } // Function 25 - Function that encapsulates all of the background processes. Originally this was in the periodic function, // but since it is used in the direct and the guidance navigation modes of the state machine, I made it a callable function // for convenience -void background_processes_16bit(uint8_t save_images_flag) +void background_processes_16bit(uint8_t _save_images_flag) { //Background processes // 1. Converting left and right image to 8bit grayscale for further processing @@ -1497,7 +1489,7 @@ void background_processes_16bit(uint8_t save_images_flag) SBM_OCV(&img_disparity_int16_cropped, &img_left_int8, &img_right_int8, N_disparities, block_size_disparities, 1);// Creating cropped disparity map image // For report: creating image for saving 1 - if (save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img1_post_SBM_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img1_post_SBM_16bit.bmp", heat_map_type);} //printf("maximum_intensity = %d\n", maximum_intensity(&img_disparity_int16_cropped)); @@ -1518,30 +1510,30 @@ void background_processes_16bit(uint8_t save_images_flag) closing_OCV(&img_disparity_int16_cropped, &img_disparity_int16_cropped, SE_closing_OCV, 1); // For report: creating image for saving 3 - if (save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img2_post_closing_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img2_post_closing_16bit.bmp", heat_map_type);} opening_OCV(&img_disparity_int16_cropped, &img_disparity_int16_cropped, SE_opening_OCV, 1); // For report: creating image for saving 2 - if (save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img3_post_opening_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img3_post_opening_16bit.bmp", heat_map_type);} dilation_OCV(&img_disparity_int16_cropped, &img_disparity_int16_cropped, SE_dilation_OCV_1, 1); // For report: creating image for saving 4 - if (save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img4_post_dilation_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img4_post_dilation_16bit.bmp", heat_map_type);} // 4. Depth image disp_to_depth_img(&img_disparity_int16_cropped, &img_depth_int16_cropped); // For report: creating image for saving 4 - if (save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img5_post_depth_16bit.bmp", heat_map_type);} + if (_save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img5_post_depth_16bit.bmp", heat_map_type);} // 5. Sobel edge detection sobel_OCV(&img_depth_int16_cropped, &img_edges_int8_cropped, SE_sobel_OCV, threshold_edge_magnitude); // For report: creating image for saving 5 - if (save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img6_post_sobel_8bit.bmp");} + if (_save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img6_post_sobel_8bit.bmp");} // 6. Morphological operations 2 // This is needed so that when using the edges as filters (to work on depth values diff --git a/sw/airborne/modules/wedgebug/wedgebug.h b/sw/airborne/modules/wedgebug/wedgebug.h index e13f4ab98f..6067f550b8 100644 --- a/sw/airborne/modules/wedgebug/wedgebug.h +++ b/sw/airborne/modules/wedgebug/wedgebug.h @@ -93,13 +93,12 @@ extern uint8_t save_images_flag; // Global functions -extern void post_disparity_crop_rect(struct crop_t *img_cropped_info, struct img_size_t *original_img_dims, +extern void post_disparity_crop_rect(struct crop_t *_img_cropped_info, struct img_size_t *_original_img_dims, const int disp_n, const int block_size); -extern void set_state(uint8_t state, uint8_t change_allowed); +extern void set_state(uint8_t _state, uint8_t change_allowed); void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type); extern void kernel_free(struct kernel_C1 *kernel); extern uint8_t getMedian(uint8_t *a, uint32_t n); -extern int heat_map_type;