Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
wedgebug.h File Reference
#include <stdint.h>
#include "modules/computer_vision/lib/vision/image.h"
+ Include dependency graph for wedgebug.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  kernel_C1
 

Functions

void wedgebug_init (void)
 
void wedgebug_periodic (void)
 
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)
 

Variables

int N_disparities
 Variable that saves previous mode to control the drone, for some memory. More...
 
int block_size_disparities
 Number of disparity levels (0-this number) More...
 
int min_disparity
 Block size used for the block matching (SBM) function. More...
 
int max_disparity
 
uint16_t K_median_h
 Width of kernel for the median kernel. More...
 
uint16_t K_median_w
 SE size for the Sobel operation, to detect edges. More...
 
int SE_opening_OCV
 
int SE_closing_OCV
 SE size for the opening operation. More...
 
int SE_dilation_OCV_1
 SE size for the closing operation. More...
 
int SE_dilation_OCV_2
 SE size for the first dilation operation. More...
 
int SE_erosion_OCV
 SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" ) More...
 
uint16_t threshold_median_depth
 Below this distance (in meters) it is considered that the robot has reached the goal, in DIRECT_CONTROL mode. More...
 
uint16_t threshold_depth_of_edges
 Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close) More...
 
int threshold_edge_magnitude
 Below this depth (cm) edges are eligible for the WedgeBug algorith. More...
 
float threshold_distance_to_goal
 Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away) More...
 
float threshold_distance_to_angle
 Below this distance (in meters) it is considered that the robot has reached the goal. More...
 
int16_t max_obstacle_confidence
 This is the confidence that no edge was found. More...
 
int16_t max_free_path_confidence
 This is the max confidence that an obstacle was spotted. More...
 
int16_t max_position_confidence
 This is the max confidence that an obstacle was not spotted. More...
 
int16_t max_heading_confidence
 This is the max confidence that a specific position was reached. More...
 
int16_t max_edge_found_micro_confidence
 This is the max confidence that a specific heading was reached. More...
 
int16_t max_edge_found_macro_confidence
 This is the max confidence that edges (micro-see above) were found. More...
 
int16_t max_no_edge_found_confidence
 This is the max confidence that edges (macro-see above were found. More...
 
int heat_map_type
 Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state) More...
 
uint8_t save_images_flag
 Set to 1 if control mode of drone is changed, 0 otherwise. More...
 

Detailed Description

Author
Ralph Rudi schmidt ralph.nosp@m..r.s.nosp@m.chmid.nosp@m.t@ou.nosp@m.tlook.nosp@m..com An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision.

Note.

  1. Information on different flight modes (such as AP_MODE_GUIDED) can be found here paparazzi/sw/airborne/firmwares/rotorcraft/autopilot_static.h
  2. When starting paparazzi and joystick is selected, the autopilot will start in the mode AP_MODE_ATTITUDE_DIRECT 4 (but engines are off)
  3. When pressing circle on the dual shock 4 controller you activate the manual control mode AP_MODE_ATTITUDE_Z_HOLD 9
  4. When pressing x on the dual shock 4 controller you activate the manual control mode AP_MODE_ATTITUDE_DIRECT 4
Author
Ralph Rudi schmidt ralph.nosp@m..r.s.nosp@m.chmid.nosp@m.t@ou.nosp@m.tlook.nosp@m..com An integration of the WegdeBug algorithm (Laubach 1999) for path finding, for drones with stereo vision.

Definition in file wedgebug.h.


Data Structure Documentation

struct kernel_C1

Definition at line 42 of file wedgebug.h.

Data Fields
uint32_t buf_size Size of values of weight buffer and values buffer.
void * buf_values Kernel value buffer. These are the values underneath the kernel.
void * buf_weights Kernel weight buffer.
uint16_t h Kernel height.
enum image_type type
uint16_t w

Function Documentation

uint8_t getMedian ( uint8_t a,
uint32_t  n 
)

Definition at line 477 of file wedgebug.c.

Referenced by median_disparity_to_point().

+ Here is the caller graph for this function:

void kernel_create ( struct kernel_C1 kernel,
uint16_t  width,
uint16_t  height,
enum image_type  type 
)

Definition at line 435 of file wedgebug.c.

References kernel_C1::buf_size, kernel_C1::buf_values, kernel_C1::buf_weights, kernel_C1::h, IMAGE_GRADIENT, IMAGE_INT16, IMAGE_JPEG, IMAGE_YUV422, kernel_C1::type, image_t::type, and kernel_C1::w.

Referenced by wedgebug_init().

+ Here is the caller graph for this function:

void kernel_free ( struct kernel_C1 kernel)

Definition at line 463 of file wedgebug.c.

References kernel_C1::buf_values, and kernel_C1::buf_weights.

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 
)

Definition at line 409 of file wedgebug.c.

References img_size_t::h, crop_t::h, img_size_t::w, crop_t::w, crop_t::x, and crop_t::y.

Referenced by SBM_OCV(), and wedgebug_init().

+ Here is the caller graph for this function:

void set_state ( uint8_t  state,
uint8_t  change_allowed 
)

Definition at line 428 of file wedgebug.c.

References current_state, and state.

Referenced by wedgebug_periodic().

+ Here is the caller graph for this function:

void wedgebug_init ( void  )

Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

Below this depth (cm) edges are eligible for the WedgeBug algorithm

Definition at line 1592 of file wedgebug.c.

References allow_state_change_EDGE_SCAN, allow_state_change_MOVE_TO_EDGE, allow_state_change_MOVE_TO_GOAL, allow_state_change_POSITION_EDGE, allow_state_change_POSITION_GOAL, allow_state_change_WEDGEBUG_START, block_size_disparities, kernel_C1::buf_size, c_img, c_img_cropped, clock_background_processes, clock_FSM, clock_total_time, clock_total_time_current, clock_total_time_previous, copy_left_img_func(), copy_right_img_func(), cv_add_to_device(), distance_robot_edge_goal, distance_traveled, edge_found_macro_confidence, edge_search_area, free_path_confidence, kernel_C1::h, image_t::h, img_size_t::h, crop_t::h, heading_confidence, heat_map_type, image_create(), IMAGE_GRAYSCALE, IMAGE_INT16, IMAGE_YUV422, img_cropped_dims, img_cropped_info, img_depth_int16_cropped, img_dims, img_disparity_int16_cropped, img_disparity_int8_cropped, img_edges_int8_cropped, img_left, img_left_int8, img_left_int8_cropped, img_middle_int16_cropped, img_middle_int8_cropped, img_right, img_right_int8, initial_heading, ES_angles::initiated, is_edge_found_macro_flag, is_edge_found_micro_flag, is_heading_reached_flag, ES_angles::is_left_reached_flag, is_mode_changed_flag, is_no_edge_found_flag, is_obstacle_detected_flag, is_path_free_flag, ES_angles::is_right_reached_flag, is_setpoint_reached_flag, is_start_reached_flag, is_state_changed_flag, is_total_timer_on_flag, K_median_h, K_median_w, kernel_create(), kernel_median_dims, FloatRMat::m, max_edge_found_macro_confidence, max_edge_found_micro_confidence, max_free_path_confidence, max_heading_confidence, max_no_edge_found_confidence, max_obstacle_confidence, max_position_confidence, median_kernel, median_kernel16bit, N_disparities, no_edge_found_confidence, number_of_states, obstacle_confidence, position_confidence, post_disparity_crop_rect(), previous_mode, previous_state, principal_points(), Rcr, Rwnedwenu, save_images_flag, SE_closing_OCV, SE_dilation_OCV_1, SE_dilation_OCV_2, SE_erosion_OCV, SE_opening_OCV, SE_sobel_OCV, threshold_depth_of_edges, threshold_distance_to_angle, threshold_distance_to_goal, threshold_distance_to_goal_direct, threshold_distance_to_goal_manual, threshold_edge_magnitude, threshold_median_depth, kernel_C1::type, Va_to_Vb(), VCr, VDISTANCEPOSITIONwned, VGOALwenu, VGOALwned, VNEDwenu, VSTARTwenu, VSTARTwned, kernel_C1::w, image_t::w, img_size_t::w, crop_t::w, WaypointAlt, WaypointX, WaypointY, WEDGEBUG_CAMERA_LEFT_FPS, WEDGEBUG_CAMERA_RIGHT_FPS, FloatVect3::x, point_t::x, crop_t::x, FloatVect3::y, point_t::y, crop_t::y, and FloatVect3::z.

+ Here is the call graph for this function:

void wedgebug_periodic ( void  )

Definition at line 1845 of file wedgebug.c.

References allow_state_change_EDGE_SCAN, allow_state_change_MOVE_TO_EDGE, allow_state_change_MOVE_TO_GOAL, allow_state_change_POSITION_EDGE, allow_state_change_WEDGEBUG_START, AP_MODE_ATTITUDE_DIRECT, AP_MODE_ATTITUDE_Z_HOLD, AP_MODE_GUIDED, AP_MODE_NAV, AUTONOMOUS_GUIDED, AUTONOMOUS_NAV, autopilot_get_mode(), autopilot_guided_goto_ned(), background_processes_16bit(), c_img_cropped, clock_background_processes, clock_FSM, clock_total_time, clock_total_time_current, clock_total_time_previous, counter_cycles, counter_state, current_mode, current_state, DIRECT_CONTROL, distance_robot_edge_goal, distance_traveled, edge_found_macro_confidence, EDGE_SCAN, edge_search_area, find_best_edge_coordinates2(), FLOAT_ANGLE_NORMALIZE, float_vect3_norm_two_points(), float_vect_copy(), free_path_confidence, guidance_h_hover_enter(), guidance_h_set_guided_heading(), guidance_h_set_guided_pos(), guidance_v_set_guided_z(), heading, heading_confidence, ES_angles::heading_initial, ES_angles::heading_max_left, ES_angles::heading_max_right, heading_towards_setpoint_WNED(), heading_towards_waypoint(), heat_map_type, img_depth_int16_cropped, img_disparity_int8_cropped, img_edges_int8_cropped, img_left_int8, img_middle_int8_cropped, img_right_int8, initial_heading, ES_angles::initiated, is_edge_found_macro_flag, is_edge_found_micro_flag, is_heading_reached(), is_heading_reached_flag, ES_angles::is_left_reached_flag, is_mode_changed_flag, is_no_edge_found_flag, is_obstacle_detected_flag, is_path_free_flag, ES_angles::is_right_reached_flag, is_setpoint_reached(), is_setpoint_reached_flag, is_start_reached_flag, is_state_changed_flag, FloatRMat::m, max_edge_found_macro_confidence, max_edge_found_micro_confidence, max_edge_search_angle, max_free_path_confidence, max_heading_confidence, max_no_edge_found_confidence, max_obstacle_confidence, max_position_confidence, median_depth_in_front, median_depth_to_point(), median_kernel16bit, MOVE_TO_EDGE, MOVE_TO_GOAL, no_edge_found_confidence, number_of_states, obstacle_confidence, position_confidence, POSITION_EDGE, POSITION_GOAL, previous_mode, previous_state, FloatEulers::psi, Rcr, Rrwned, Rwnedwenu, save_image_gray(), save_image_HM(), save_images_flag, set_state(), stateGetNedToBodyEulers_f(), stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), threshold_depth_of_edges, threshold_distance_to_angle, threshold_distance_to_goal, threshold_distance_to_goal_direct, threshold_median_depth, time_state, Va_to_Vb(), Vb_to_Va(), VCr, VDISTANCEPOSITIONwned, VEDGECOORDINATESc, VEDGECOORDINATESr, VEDGECOORDINATESwenu, VEDGECOORDINATESwned, VGOALc, VGOALr, VGOALwned, VHOLDINGPOINTwned, VNEDwenu, VPBESTEDGECOORDINATESwned, VRwned, WEDGEBUG_START, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

+ Here is the call graph for this function:

Variable Documentation

int block_size_disparities

Number of disparity levels (0-this number)

Definition at line 256 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

int heat_map_type

Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state)

Definition at line 102 of file wedgebug.h.

Referenced by background_processes(), background_processes_16bit(), wedgebug_init(), and wedgebug_periodic().

uint16_t K_median_h

Width of kernel for the median kernel.

Definition at line 113 of file wedgebug.c.

Referenced by wedgebug_init().

uint16_t K_median_w

SE size for the Sobel operation, to detect edges.

Definition at line 112 of file wedgebug.c.

Referenced by wedgebug_init().

int max_disparity
int16_t max_edge_found_macro_confidence

This is the max confidence that edges (micro-see above) were found.

Definition at line 166 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_edge_found_micro_confidence

This is the max confidence that a specific heading was reached.

Definition at line 165 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_free_path_confidence

This is the max confidence that an obstacle was spotted.

Definition at line 162 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_heading_confidence

This is the max confidence that a specific position was reached.

Definition at line 164 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_no_edge_found_confidence

This is the max confidence that edges (macro-see above were found.

Definition at line 167 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_obstacle_confidence

This is the confidence that no edge was found.

Definition at line 161 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t max_position_confidence

This is the max confidence that an obstacle was not spotted.

Definition at line 163 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int min_disparity

Block size used for the block matching (SBM) function.

Definition at line 257 of file wedgebug.c.

Referenced by CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), CN_vector_escape_velocity(), and CN_vector_velocity().

int N_disparities

Variable that saves previous mode to control the drone, for some memory.

Definition at line 255 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

uint8_t save_images_flag

Set to 1 if control mode of drone is changed, 0 otherwise.

Definition at line 180 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int SE_closing_OCV

SE size for the opening operation.

Definition at line 107 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

int SE_dilation_OCV_1

SE size for the closing operation.

Definition at line 108 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

int SE_dilation_OCV_2

SE size for the first dilation operation.

Definition at line 109 of file wedgebug.c.

Referenced by wedgebug_init().

int SE_erosion_OCV

SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )

Definition at line 110 of file wedgebug.c.

Referenced by background_processes_16bit(), and wedgebug_init().

int SE_opening_OCV

Definition at line 106 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

uint16_t threshold_depth_of_edges

Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

Below this median depth (cm), an obstacle is considered to block the way (i.e. the blocking obstacle needs to be close)

Definition at line 150 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

float threshold_distance_to_angle

Below this distance (in meters) it is considered that the robot has reached the goal.

Definition at line 146 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

float threshold_distance_to_goal

Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away)

Definition at line 145 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int threshold_edge_magnitude

Below this depth (cm) edges are eligible for the WedgeBug algorith.

Below this depth (cm) edges are eligible for the WedgeBug algorith.

Definition at line 142 of file wedgebug.c.

Referenced by background_processes(), background_processes_16bit(), and wedgebug_init().

uint16_t threshold_median_depth

Below this distance (in meters) it is considered that the robot has reached the goal, in DIRECT_CONTROL mode.

Definition at line 149 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().