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.c File Reference
#include <stdio.h>
#include "modules/wedgebug/wedgebug.h"
#include "modules/wedgebug/wedgebug_opencv.h"
#include "modules/computer_vision/cv.h"
#include "modules/computer_vision/lib/vision/image.h"
#include "pthread.h"
#include <stdint.h>
#include "state.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra.h"
#include "math/pprz_geodetic_float.h"
#include "generated/flight_plan.h"
#include "firmwares/rotorcraft/autopilot_guided.h"
#include <math.h>
#include "autopilot.h"
#include <time.h>
+ Include dependency graph for wedgebug.c:

Go to the source code of this file.

Data Structures

struct  ES_angles
 

Macros

#define WEDGEBUG_CAMERA_RIGHT_FPS   0
 
#define WEDGEBUG_CAMERA_LEFT_FPS   0
 

Enumerations

enum  navigation_state {
  MOVE_TO_GOAL = 1, POSITION_GOAL = 2, WEDGEBUG_START = 3, MOVE_TO_EDGE = 4,
  POSITION_EDGE = 5, EDGE_SCAN = 6
}
 Camera focal length, in pixels (i.e. distance between camera. More...
 
enum  control_mode_state { DIRECT_CONTROL = 1, AUTONOMOUS_NAV = 2, AUTONOMOUS_GUIDED = 3 }
 

Functions

const char * get_img_type (enum image_type img_type)
 
void show_image_data (struct image_t *img)
 
void show_image_entry (struct image_t *img, int entry_position, const char *img_name)
 
void show_rotation_matrix (struct FloatRMat *R)
 
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)
 
static struct image_tcopy_left_img_func (struct image_t *img)
 
static struct image_tcopy_right_img_func (struct image_t *img)
 
void UYVYs_interlacing_V (struct image_t *YY, struct image_t *left, struct image_t *right)
 
void UYVYs_interlacing_H (struct image_t *merged, struct image_t *left, struct image_t *right)
 
uint32_t maximum_intensity (struct image_t *img)
 
void thresholding_img (struct image_t *img, uint8_t threshold)
 
void principal_points (struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info)
 
float disp_to_depth (const uint8_t d, const float b, const uint16_t f)
 
uint8_t depth_to_disp (const float depth, 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)
 
int32_t indx1d_a (const int32_t y, const int32_t x, const struct image_t *img)
 
int32_t indx1d_b (const int32_t y, const int32_t x, const struct img_size_t *img_dims)
 
int32_t indx1d_c (const int32_t y, const int32_t x, const uint16_t img_height, const uint16_t img_width)
 
void Va_to_Vb (struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa)
 
void Vb_to_Va (struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa)
 
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)
 
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)
 
float float_vect3_norm_two_points (struct FloatVect3 *V1, struct FloatVect3 *V2)
 
float heading_towards_waypoint (uint8_t wp)
 
float heading_towards_setpoint_WNED (struct FloatVect3 *VSETPOINTwned)
 
uint8_t median_disparity_to_point (struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
 
uint8_t find_best_edge_coordinates (struct FloatVect3 *VEDGECOORDINATESc, struct FloatVect3 *VTARGETc, struct image_t *img_edges, struct image_t *img_disparity, struct crop_t *edge_search_area, uint8_t threshold, int16_t max_confidence)
 
uint8_t is_setpoint_reached (struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold)
 
float float_norm_two_angles (float target_angle, float current_angle)
 
uint8_t is_heading_reached (float target_angle, float current_angle, float threshold)
 
uint8_t are_setpoint_and_angle_reached (struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold_setpoint, float target_angle, float current_angle, float threshold_angle)
 
void disp_to_depth_img (struct image_t *img8bit_input, struct image_t *img16bit_output)
 
void background_processes (uint8_t save_images_flag)
 
uint16_t getMedian16bit (uint16_t *a, uint32_t n)
 
float dispfixed_to_disp (const int16_t d)
 
float disp_to_depth_16bit (const int16_t d, const float b, const uint16_t f)
 
void Vi_to_Vc_depth (struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const float depth, 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)
 
uint16_t median_depth_to_point (struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
 
uint8_t find_best_edge_coordinates2 (struct FloatVect3 *VEDGECOORDINATESc, struct FloatVect3 *VTARGETc, struct image_t *img_edges, struct image_t *img_depth, struct crop_t *edge_search_area, uint16_t threshold, int16_t max_confidence)
 
void background_processes_16bit (uint8_t save_images_flag)
 
void wedgebug_init ()
 
void wedgebug_periodic ()
 

Variables

struct image_t img_left
 
struct image_t img_right
 Image obtained from left camera (UYVY format) More...
 
struct image_t img_left_int8
 Image obtained from right camera (UYVY format) More...
 
struct image_t img_left_int8_cropped
 Image obtained from left camera, converted into 8bit gray image. More...
 
struct image_t img_right_int8
 
struct image_t img_disparity_int8_cropped
 Image obtained from right camera, converted into 8bit gray image. More...
 
struct image_t img_depth_int16_cropped
 Image obtained after simple block matching. More...
 
struct image_t img_middle_int8_cropped
 Image holding depth values (cm) obtained from the disparity image. More...
 
struct image_t img_edges_int8_cropped
 
struct image_t img_disparity_int16_cropped
 Image obtained from the external sobel edge detection function = sobel_OCV. More...
 
struct image_t img_middle_int16_cropped
 
struct crop_t img_cropped_info
 
struct img_size_t img_dims
 
struct img_size_t img_cropped_dims
 Dimensions of images captured by the camera (left and right have same dimension) More...
 
struct img_size_t kernel_median_dims
 Dimension of image after it has been cropped to remove pixel error due to block matching limitations. More...
 
struct kernel_C1 median_kernel
 Dimensions of the kernel that detect median disparity in front of drone (for obstacle detection) More...
 
struct kernel_C1 median_kernel16bit
 
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...
 
int SE_sobel_OCV
 SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE", its needed to "drag" the depth of the foreground objects over the edges detected) More...
 
uint16_t K_median_w
 SE size for the Sobel operation, to detect edges. More...
 
uint16_t K_median_h
 Width of kernel for the median kernel. More...
 
struct FloatVect3 VSTARTwenu
 Height of kernel for the median kernel. More...
 
struct FloatVect3 VSTARTwned
 Declared vector of coordinates of start position in ENU world coordinate system. More...
 
struct FloatVect3 VGOALwenu
 Declared vector of coordinates of start position in NED world coordinate system. More...
 
struct FloatVect3 VGOALwned
 Declared vector of coordinates of goal in ENU world coordinate system. More...
 
struct FloatVect3 VGOALr
 Declared vector of coordinates of goal in NED world coordinate system. More...
 
struct FloatVect3 VGOALc
 Declared vector of coordinates of goal in robot coordinate system. More...
 
struct FloatVect3 VEDGECOORDINATESc
 Declared vector of coordinates of goal in camera coordinate system. More...
 
struct FloatVect3 VEDGECOORDINATESr
 Declared vector of coordinates of "best" edge detected in camera coordinate system. More...
 
struct FloatVect3 VEDGECOORDINATESwned
 Declared vector of coordinates of "best" edge detected in robot coordinate system. More...
 
struct FloatVect3 VEDGECOORDINATESwenu
 Declared vector of coordinates of "best" edge detected in NED world coordinate system. More...
 
struct FloatVect3 VDISTANCEPOSITIONwned
 Declared vector of coordinates of "best" edge detected in ENU world coordinate system. More...
 
struct FloatVect3 VHOLDINGPOINTwned
 Declared a vector to hold the current position, which is needed for calculating the distance traveled. More...
 
struct FloatVect3 VPBESTEDGECOORDINATESwned
 Declared a vector to hold the position of a holding point (offten used to make sure drone stays still before stuff happens) More...
 
int threshold_edge_magnitude
 Declared vector of coordinates of previous "best" edge detected in NED world coordinate system. More...
 
uint8_t threshold_median_disparity
 Edges with a magnitude above this value are detected. Above this value, edges are given the value 127, otherwise they are given the value zero. More...
 
uint8_t threshold_disparity_of_edges
 Above this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle need to be close) 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...
 
float threshold_distance_to_goal_direct
 Below this distance (in radians) it is considered that the robot has reached the target angle. 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 disparity, an obstacle is considered to block the way (i.e. the blocking obstacle need to be close) More...
 
int16_t obstacle_confidence
 Below this depth (m) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away) More...
 
int16_t free_path_confidence
 This is the confidence that an obstacle was spotted. More...
 
int16_t position_confidence
 This is the confidence that no obstacle was spotted. More...
 
int16_t heading_confidence
 This is the confidence that the desired position was reached. More...
 
int16_t edge_found_macro_confidence
 This is the confidence that the desired heading is reached. More...
 
int16_t no_edge_found_confidence
 This is the confidence that an edge was found - outside of the find_best_edge_coordinates function. 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...
 
uint8_t is_start_reached_flag
 This is the max confidence that no edges were found. More...
 
uint8_t is_setpoint_reached_flag
 
uint8_t is_obstacle_detected_flag
 Set to 1 if setpoint is reached, 0 otherwise. More...
 
uint8_t is_path_free_flag
 Set to 1 if obstacle is detected, 0 otherwise. More...
 
uint8_t is_heading_reached_flag
 Set to 1 if no obstacle is detected, 0 otherwise. More...
 
uint8_t is_edge_found_macro_flag
 Set to 1 if heading is reached, 0 otherwise. More...
 
uint8_t is_edge_found_micro_flag
 Set to 1 if best edge (according to macro confidence) was found, 0 otherwise. More...
 
uint8_t is_no_edge_found_flag
 Set to 1 if best edge (according to micro confidence) was found, 0 otherwise. More...
 
uint8_t is_state_changed_flag
 Set to 1 if no edge was identified, 0 otherwise. More...
 
uint8_t is_mode_changed_flag
 Set to 1 if state was changed, 0 otherwise. More...
 
uint8_t save_images_flag
 Set to 1 if control mode of drone is changed, 0 otherwise. More...
 
struct point_t c_img
 
struct point_t c_img_cropped
 Principal point of normal camera images. More...
 
struct crop_t edge_search_area
 Principal point of cropped camera images. More...
 
struct FloatRMat Rwnedwenu
 This structure holds information about the window in which edges are searched in. More...
 
struct FloatVect3 VNEDwenu
 
struct FloatRMat Rrwned
 
struct FloatVect3 VRwned
 
struct FloatRMat Rcr
 
struct FloatVect3 VCr
 
float b
 
uint16_t f = WEDGEBUG_CAMERA_FOCAL_LENGTH
 Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) More...
 
enum navigation_state current_state
 
enum control_mode_state current_mode
 
struct ES_angles initial_heading
 
double time_state [NUMBER_OF_STATES]
 
double counter_state [NUMBER_OF_STATES]
 
double counter_cycles
 
clock_t clock_total_time
 
clock_t clock_total_time_current
 
clock_t clock_total_time_previous
 
clock_t clock_background_processes
 
clock_t clock_FSM
 
uint8_t previous_state
 
uint8_t previous_mode
 Variable that saves previous state the state machine was in, for some memory. More...
 
int N_disparities = 64
 Variable that saves previous mode to control the drone, for some memory. More...
 
int block_size_disparities = 25
 Number of disparity levels (0-this number) More...
 
int min_disparity = 0
 Block size used for the block matching (SBM) function. More...
 
float heading
 
float max_edge_search_angle
 Variable for storing the heading of the drone (psi in radians) More...
 
uint8_t median_disparity_in_front
 The maximum angle (in adians) to the left and right of the drone, that edges can be detected in. Edges outside of this area are considered to be in a minimum. More...
 
uint16_t median_depth_in_front
 Variable to hold the median disparity in front of the drone. Needed to see if obstacle is there. More...
 
float distance_traveled
 Variable to hold the median depth in front of the drone. Needed to see if obstacle is there. More...
 
uint8_t number_of_states
 
float distance_robot_edge_goal
 
int heat_map_type
 Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state) More...
 
uint8_t allow_state_change_MOVE_TO_GOAL
 
uint8_t allow_state_change_POSITION_GOAL
 
uint8_t allow_state_change_WEDGEBUG_START
 
uint8_t allow_state_change_MOVE_TO_EDGE
 
uint8_t allow_state_change_POSITION_EDGE
 
uint8_t allow_state_change_EDGE_SCAN
 
uint8_t is_total_timer_on_flag
 
float threshold_distance_to_goal_manual
 

Data Structure Documentation

struct ES_angles

Definition at line 231 of file wedgebug.c.

Data Fields
float heading_initial
float heading_max_left
float heading_max_right
uint8_t initiated
uint8_t is_left_reached_flag
uint8_t is_right_reached_flag

Macro Definition Documentation

#define WEDGEBUG_CAMERA_LEFT_FPS   0

Definition at line 65 of file wedgebug.c.

Referenced by wedgebug_init().

#define WEDGEBUG_CAMERA_RIGHT_FPS   0

Definition at line 62 of file wedgebug.c.

Referenced by wedgebug_init().

Enumeration Type Documentation

Enumerator
DIRECT_CONTROL 
AUTONOMOUS_NAV 
AUTONOMOUS_GUIDED 

Definition at line 220 of file wedgebug.c.

Camera focal length, in pixels (i.e. distance between camera.

Enumerator
MOVE_TO_GOAL 
POSITION_GOAL 
WEDGEBUG_START 
MOVE_TO_EDGE 
POSITION_EDGE 
EDGE_SCAN 

Definition at line 209 of file wedgebug.c.

Function Documentation

uint8_t are_setpoint_and_angle_reached ( struct FloatVect3 VGOAL,
struct FloatVect3 VCURRENTPOSITION,
float  threshold_setpoint,
float  target_angle,
float  current_angle,
float  threshold_angle 
)

Definition at line 1382 of file wedgebug.c.

References float_norm_two_angles(), and float_vect3_norm_two_points().

+ Here is the call graph for this function:

static struct image_t * copy_left_img_func ( struct image_t img)
static

Definition at line 543 of file wedgebug.c.

References image_copy(), and img_left.

Referenced by wedgebug_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static struct image_t * copy_right_img_func ( struct image_t img)
static

Definition at line 553 of file wedgebug.c.

References image_copy(), and img_right.

Referenced by wedgebug_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

uint8_t depth_to_disp ( const float  depth,
const float  b,
const uint16_t  f 
)

Definition at line 720 of file wedgebug.c.

float disp_to_depth ( const uint8_t  d,
const float  b,
const uint16_t  f 
)

Definition at line 699 of file wedgebug.c.

Referenced by disp_to_depth_img(), and Vi_to_Vc().

+ Here is the caller graph for this function:

float disp_to_depth_16bit ( const int16_t  d,
const float  b,
const uint16_t  f 
)

Definition at line 713 of file wedgebug.c.

References dispfixed_to_disp().

Referenced by disp_to_depth_img(), and Vi_to_Vc16bit().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void disp_to_depth_img ( struct image_t img8bit_input,
struct image_t img16bit_output 
)

Definition at line 1393 of file wedgebug.c.

References b, image_t::buf, disp_to_depth(), disp_to_depth_16bit(), f, image_t::h, IMAGE_GRAYSCALE, IMAGE_INT16, image_t::type, and image_t::w.

Referenced by background_processes(), and background_processes_16bit().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

float dispfixed_to_disp ( const int16_t  d)

Definition at line 706 of file wedgebug.c.

Referenced by disp_to_depth_16bit().

+ Here is the caller graph for this function:

uint8_t find_best_edge_coordinates ( struct FloatVect3 VEDGECOORDINATESc,
struct FloatVect3 VTARGETc,
struct image_t img_edges,
struct image_t img_disparity,
struct crop_t edge_search_area,
uint8_t  threshold,
int16_t  max_confidence 
)
uint8_t find_best_edge_coordinates2 ( struct FloatVect3 VEDGECOORDINATESc,
struct FloatVect3 VTARGETc,
struct image_t img_edges,
struct image_t img_depth,
struct crop_t edge_search_area,
uint16_t  threshold,
int16_t  max_confidence 
)

Definition at line 1241 of file wedgebug.c.

References image_t::buf, c_img_cropped, f, float_vect3_norm_two_points(), crop_t::h, indx1d_a(), Vi_to_Vc_depth(), crop_t::w, FloatVect3::x, point_t::x, crop_t::x, FloatVect3::y, point_t::y, crop_t::y, and FloatVect3::z.

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

float float_norm_two_angles ( float  target_angle,
float  current_angle 
)

Definition at line 1364 of file wedgebug.c.

Referenced by are_setpoint_and_angle_reached(), and is_heading_reached().

+ Here is the caller graph for this function:

float float_vect3_norm_two_points ( struct FloatVect3 V1,
struct FloatVect3 V2 
)

Definition at line 978 of file wedgebug.c.

References float_vect3_norm(), and VECT3_DIFF.

Referenced by are_setpoint_and_angle_reached(), find_best_edge_coordinates(), find_best_edge_coordinates2(), is_setpoint_reached(), and wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

const char * get_img_type ( enum image_type  img_type)

Definition at line 365 of file wedgebug.c.

References IMAGE_GRADIENT, IMAGE_GRAYSCALE, IMAGE_JPEG, and IMAGE_YUV422.

Referenced by show_image_data().

+ Here is the caller graph for this function:

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:

uint16_t getMedian16bit ( uint16_t a,
uint32_t  n 
)

Definition at line 508 of file wedgebug.c.

Referenced by median_depth_to_point().

+ Here is the caller graph for this function:

float heading_towards_setpoint_WNED ( struct FloatVect3 VSETPOINTwned)

Definition at line 999 of file wedgebug.c.

References stateGetPositionNed_f(), VECT2_DIFF, FloatVect2::x, and FloatVect2::y.

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

float heading_towards_waypoint ( uint8_t  wp)

Definition at line 986 of file wedgebug.c.

References stateGetPositionEnu_f(), VECT2_DIFF, WaypointX, WaypointY, FloatVect2::x, and FloatVect2::y.

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

int32_t indx1d_a ( const int32_t  y,
const int32_t  x,
const struct image_t img 
)

Definition at line 815 of file wedgebug.c.

References image_t::h, and image_t::w.

Referenced by find_best_edge_coordinates(), find_best_edge_coordinates2(), median_depth_to_point(), and median_disparity_to_point().

+ Here is the caller graph for this function:

int32_t indx1d_b ( const int32_t  y,
const int32_t  x,
const struct img_size_t img_dims 
)

Definition at line 830 of file wedgebug.c.

References img_size_t::h, and img_size_t::w.

int32_t indx1d_c ( const int32_t  y,
const int32_t  x,
const uint16_t  img_height,
const uint16_t  img_width 
)

Definition at line 844 of file wedgebug.c.

Referenced by median_depth_to_point(), and median_disparity_to_point().

+ Here is the caller graph for this function:

uint8_t is_heading_reached ( float  target_angle,
float  current_angle,
float  threshold 
)

Definition at line 1375 of file wedgebug.c.

References float_norm_two_angles().

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

uint8_t is_setpoint_reached ( struct FloatVect3 VGOAL,
struct FloatVect3 VCURRENTPOSITION,
float  threshold 
)

Definition at line 1356 of file wedgebug.c.

References float_vect3_norm_two_points().

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ 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.

uint32_t maximum_intensity ( struct image_t img)
uint16_t median_depth_to_point ( struct point_t Vi,
struct image_t img,
struct kernel_C1 kernel_median 
)

Definition at line 1067 of file wedgebug.c.

References image_t::buf, kernel_C1::buf_values, getMedian16bit(), kernel_C1::h, image_t::h, indx1d_a(), indx1d_c(), median_kernel, kernel_C1::w, image_t::w, point_t::x, and point_t::y.

Referenced by wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

uint8_t median_disparity_to_point ( struct point_t Vi,
struct image_t img,
struct kernel_C1 kernel_median 
)

Definition at line 1011 of file wedgebug.c.

References image_t::buf, kernel_C1::buf_values, getMedian(), kernel_C1::h, image_t::h, indx1d_a(), indx1d_c(), median_kernel, kernel_C1::w, image_t::w, point_t::x, and point_t::y.

+ Here is the call graph for this function:

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 principal_points ( struct point_t c_output,
const struct point_t c_old_input,
struct crop_t img_cropped_info 
)

Definition at line 691 of file wedgebug.c.

References point_t::x, crop_t::x, point_t::y, and crop_t::y.

Referenced by 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 show_image_data ( struct image_t img)

Definition at line 378 of file wedgebug.c.

References image_t::buf, image_t::buf_size, get_img_type(), image_t::h, image_t::type, and image_t::w.

+ Here is the call graph for this function:

void show_image_entry ( struct image_t img,
int  entry_position,
const char *  img_name 
)

Definition at line 389 of file wedgebug.c.

References image_t::buf.

void show_rotation_matrix ( struct FloatRMat R)

Definition at line 395 of file wedgebug.c.

References FloatRMat::m.

void thresholding_img ( struct image_t img,
uint8_t  threshold 
)

Definition at line 675 of file wedgebug.c.

References image_t::buf, and image_t::buf_size.

void UYVYs_interlacing_H ( struct image_t merged,
struct image_t left,
struct image_t right 
)

Definition at line 602 of file wedgebug.c.

References image_t::buf, image_t::h, and image_t::w.

void UYVYs_interlacing_V ( struct image_t YY,
struct image_t left,
struct image_t right 
)

Definition at line 563 of file wedgebug.c.

References image_t::buf, image_t::h, and image_t::w.

void Va_to_Vb ( struct FloatVect3 Vb,
struct FloatVect3 Va,
struct FloatRMat Rba,
struct FloatVect3 VOa 
)

Definition at line 859 of file wedgebug.c.

References float_rmat_vmult(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by Vw_to_Vc(), wedgebug_init(), and wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void Vb_to_Va ( struct FloatVect3 Va,
struct FloatVect3 Vb,
struct FloatRMat Rba,
struct FloatVect3 VOa 
)

Definition at line 880 of file wedgebug.c.

References float_rmat_transp_vmult(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by Vc_to_Vw(), and wedgebug_periodic().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 
)

Definition at line 950 of file wedgebug.c.

References Vb_to_Va(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call graph for this function:

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 
)

Definition at line 730 of file wedgebug.c.

References disp_to_depth(), f, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by find_best_edge_coordinates().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 
)

Definition at line 782 of file wedgebug.c.

References disp_to_depth_16bit(), f, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call graph for this function:

void Vi_to_Vc_depth ( struct FloatVect3 scene_point,
int32_t  image_point_y,
int32_t  image_point_x,
const float  depth,
const uint16_t  f 
)

Definition at line 763 of file wedgebug.c.

References f, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by find_best_edge_coordinates2().

+ Here is the caller graph for this function:

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 
)

Definition at line 894 of file wedgebug.c.

References FloatRMat::m, Va_to_Vb(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

+ Here is the call 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

uint8_t allow_state_change_EDGE_SCAN

Definition at line 276 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t allow_state_change_MOVE_TO_EDGE

Definition at line 274 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t allow_state_change_MOVE_TO_GOAL

Definition at line 271 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t allow_state_change_POSITION_EDGE

Definition at line 275 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t allow_state_change_POSITION_GOAL

Definition at line 272 of file wedgebug.c.

Referenced by wedgebug_init().

uint8_t allow_state_change_WEDGEBUG_START

Definition at line 273 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int block_size_disparities = 25

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().

struct point_t c_img

Definition at line 183 of file wedgebug.c.

Referenced by wedgebug_init().

struct point_t c_img_cropped

Principal point of normal camera images.

Definition at line 184 of file wedgebug.c.

Referenced by find_best_edge_coordinates(), find_best_edge_coordinates2(), wedgebug_init(), and wedgebug_periodic().

clock_t clock_background_processes

Definition at line 249 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

clock_t clock_FSM

Definition at line 250 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

clock_t clock_total_time

Definition at line 246 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

clock_t clock_total_time_current

Definition at line 247 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

clock_t clock_total_time_previous

Definition at line 248 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

double counter_cycles

Definition at line 245 of file wedgebug.c.

Referenced by wedgebug_periodic().

double counter_state[NUMBER_OF_STATES]

Definition at line 244 of file wedgebug.c.

Referenced by wedgebug_periodic().

enum control_mode_state current_mode

Definition at line 226 of file wedgebug.c.

Referenced by wedgebug_periodic().

enum navigation_state current_state

Definition at line 217 of file wedgebug.c.

Referenced by set_state(), and wedgebug_periodic().

float distance_robot_edge_goal

Definition at line 265 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

float distance_traveled

Variable to hold the median depth in front of the drone. Needed to see if obstacle is there.

Definition at line 263 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t edge_found_macro_confidence

This is the confidence that the desired heading is reached.

Definition at line 159 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct crop_t edge_search_area

Principal point of cropped camera images.

Definition at line 187 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint16_t f = WEDGEBUG_CAMERA_FOCAL_LENGTH

Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)

Definition at line 204 of file wedgebug.c.

Referenced by ac_char_update(), actuators_bebop_commit(), actuators_disco_commit(), ahrs_dcm_update_gps(), ahrs_madgwick_propagate(), baro_cb(), bat_read(), bef(), calc_fast9_lukas_kanade(), calc_lm35(), check_line(), colorfilter_func(), compute_ff_vel(), dc_send_shot_position(), detect_gate_func(), detect_gate_init(), Dhane_distortion(), discrete_ekf_new(), disp_to_depth_img(), Drift_correction(), ecef_of_lla_d(), ecef_of_lla_f(), electrical_periodic(), error_output(), esc32_get_float(), extract_information_from_parameters(), ezcurrent_read_event(), find_best_edge_coordinates(), find_best_edge_coordinates2(), float_from_buf(), float_quat_of_axis_angle(), gc_of_gd_lat_d(), get_diff(), get_humidity(), get_pitot(), get_pressure(), get_temp(), get_time_to_home(), get_vertical_thrust_coeff(), gh_set_max_speed(), guidance_v_init(), hff_init(), horizontal_ctrl_module_run(), image_labeling(), init_second_order_low_pass(), ins_alt_float_init(), ins_ekf2_publish_attitude(), ins_int_update_gps(), ins_mekf_wind_init(), ins_mekf_wind_update_incidence(), int32_vect2_normalize(), jevois_send_message(), linear_filter(), linear_measure(), link_mcu_on_can_msg(), lla_of_ecef_d(), lla_of_ecef_f(), mpu60x0_i2c_event(), mpu60x0_spi_event(), mtostk_apply_polynomial_temp(), nav_approaching(), nav_catapult_run(), nav_circle(), nav_route(), nav_set_heading_towards(), nav_survey_computefourth_corner(), nav_survey_ComputeProjectionOnLine(), nav_survey_losange_carto(), nav_survey_losange_carto_init(), nav_survey_poly_osam_setup_towards(), nav_survey_poly_setup_towards(), Normalize(), nps_autopilot_run_step(), opticflow_pmw3901_publish(), orange_avoider_guided_periodic(), orange_avoider_periodic(), parse_mavpilot_msg(), PID_divergence_control(), PID_flow_control(), pos_est_cb(), px4flow_i2c_frame_cb(), relative_localization_filter_periodic(), renorm_factor(), rtos_mon_periodic_arch(), rtp_frame_send(), runge_kutta_1_float(), runge_kutta_2_float(), runge_kutta_4_float(), sonar_vl53l1x_publish(), sonar_vl53l1x_read(), srf08_event(), stabilization_attitude_read_rc_roll_pitch_earth_quat_f(), stabilization_opticflow_vel_cb(), store_min_dist(), trilateration_compute(), trilateration_init(), undistort_image_func(), update_ae_awb(), update_biased_z_conf(), update_errors(), update_first_order_high_pass(), v_ctl_climb_loop(), vel_est_cb(), vertical_ctrl_module_run(), vff_init_zero(), vff_propagate(), vff_update_agl(), vff_update_baro_conf(), vff_update_vz_conf(), vff_update_z_conf(), Vi_to_Vc(), Vi_to_Vc16bit(), Vi_to_Vc_depth(), wgs84_ellipsoid_to_geoid_f(), wgs84_ellipsoid_to_geoid_i(), wind_estimator_periodic(), write_exif_jpeg(), xgeqrf(), and xgeqrf_f().

int16_t free_path_confidence

This is the confidence that an obstacle was spotted.

Definition at line 154 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t heading_confidence

This is the confidence that the desired position was reached.

Definition at line 156 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int heat_map_type

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

Definition at line 266 of file wedgebug.c.

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

struct img_size_t img_cropped_dims

Dimensions of images captured by the camera (left and right have same dimension)

Definition at line 96 of file wedgebug.c.

Referenced by wedgebug_init().

struct crop_t img_cropped_info

Definition at line 92 of file wedgebug.c.

Referenced by wedgebug_init().

struct image_t img_depth_int16_cropped

Image obtained after simple block matching.

Definition at line 80 of file wedgebug.c.

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

struct img_size_t img_dims

Definition at line 95 of file wedgebug.c.

Referenced by wedgebug_init().

struct image_t img_disparity_int16_cropped

Image obtained from the external sobel edge detection function = sobel_OCV.

Definition at line 84 of file wedgebug.c.

Referenced by background_processes_16bit(), and wedgebug_init().

struct image_t img_disparity_int8_cropped

Image obtained from right camera, converted into 8bit gray image.

Definition at line 79 of file wedgebug.c.

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

struct image_t img_edges_int8_cropped
struct image_t img_left
struct image_t img_left_int8

Image obtained from right camera (UYVY format)

Definition at line 76 of file wedgebug.c.

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

struct image_t img_left_int8_cropped

Image obtained from left camera, converted into 8bit gray image.

Definition at line 77 of file wedgebug.c.

Referenced by wedgebug_init().

struct image_t img_middle_int16_cropped

Definition at line 85 of file wedgebug.c.

Referenced by wedgebug_init().

struct image_t img_middle_int8_cropped

Image holding depth values (cm) obtained from the disparity image.

Definition at line 81 of file wedgebug.c.

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

struct image_t img_right

Image obtained from left camera (UYVY format)

Definition at line 75 of file wedgebug.c.

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

struct image_t img_right_int8
struct ES_angles initial_heading

Definition at line 239 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_edge_found_macro_flag

Set to 1 if heading is reached, 0 otherwise.

Definition at line 175 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_edge_found_micro_flag

Set to 1 if best edge (according to macro confidence) was found, 0 otherwise.

Definition at line 176 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_heading_reached_flag

Set to 1 if no obstacle is detected, 0 otherwise.

Definition at line 174 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_mode_changed_flag

Set to 1 if state was changed, 0 otherwise.

Definition at line 179 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_no_edge_found_flag

Set to 1 if best edge (according to micro confidence) was found, 0 otherwise.

Definition at line 177 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_obstacle_detected_flag

Set to 1 if setpoint is reached, 0 otherwise.

Definition at line 172 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_path_free_flag

Set to 1 if obstacle is detected, 0 otherwise.

Definition at line 173 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_setpoint_reached_flag

Definition at line 171 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_start_reached_flag

This is the max confidence that no edges were found.

Definition at line 170 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_state_changed_flag

Set to 1 if no edge was identified, 0 otherwise.

Definition at line 178 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t is_total_timer_on_flag

Definition at line 279 of file wedgebug.c.

Referenced by wedgebug_init().

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().

struct img_size_t kernel_median_dims

Dimension of image after it has been cropped to remove pixel error due to block matching limitations.

Definition at line 98 of file wedgebug.c.

Referenced by wedgebug_init().

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().

float max_edge_search_angle
Initial value:
= M_PI /
2

Variable for storing the heading of the drone (psi in radians)

Definition at line 259 of file wedgebug.c.

Referenced by 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().

uint16_t median_depth_in_front

Variable to hold the median disparity in front of the drone. Needed to see if obstacle is there.

Definition at line 262 of file wedgebug.c.

Referenced by wedgebug_periodic().

uint8_t median_disparity_in_front

The maximum angle (in adians) to the left and right of the drone, that edges can be detected in. Edges outside of this area are considered to be in a minimum.

Definition at line 261 of file wedgebug.c.

struct kernel_C1 median_kernel

Dimensions of the kernel that detect median disparity in front of drone (for obstacle detection)

Definition at line 102 of file wedgebug.c.

Referenced by median_depth_to_point(), median_disparity_to_point(), and wedgebug_init().

struct kernel_C1 median_kernel16bit

Definition at line 103 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int min_disparity = 0

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 = 64

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().

int16_t no_edge_found_confidence

This is the confidence that an edge was found - outside of the find_best_edge_coordinates function.

Definition at line 160 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t number_of_states

Definition at line 264 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t obstacle_confidence

Below this depth (m) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away)

Definition at line 153 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

int16_t position_confidence

This is the confidence that no obstacle was spotted.

Definition at line 155 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t previous_mode

Variable that saves previous state the state machine was in, for some memory.

Definition at line 254 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

uint8_t previous_state

Definition at line 253 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatRMat Rcr

Definition at line 198 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatRMat Rrwned

Definition at line 195 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatRMat Rwnedwenu

This structure holds information about the window in which edges are searched in.

Definition at line 192 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

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().

int SE_sobel_OCV

SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE", its needed to "drag" the depth of the foreground objects over the edges detected)

Definition at line 111 of file wedgebug.c.

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

uint16_t threshold_depth_of_edges

Below this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle need 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().

uint8_t threshold_disparity_of_edges

Above this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle need to be close)

Definition at line 144 of file wedgebug.c.

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().

float threshold_distance_to_goal_direct

Below this distance (in radians) it is considered that the robot has reached the target angle.

Definition at line 147 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

float threshold_distance_to_goal_manual

Definition at line 280 of file wedgebug.c.

Referenced by wedgebug_init().

int threshold_edge_magnitude

Declared vector of coordinates of previous "best" edge detected in NED world coordinate system.

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().

uint8_t threshold_median_disparity

Edges with a magnitude above this value are detected. Above this value, edges are given the value 127, otherwise they are given the value zero.

Definition at line 143 of file wedgebug.c.

double time_state[NUMBER_OF_STATES]

Definition at line 243 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VCr

Definition at line 199 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatVect3 VDISTANCEPOSITIONwned

Declared vector of coordinates of "best" edge detected in ENU world coordinate system.

Definition at line 133 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatVect3 VEDGECOORDINATESc

Declared vector of coordinates of goal in camera coordinate system.

Definition at line 125 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VEDGECOORDINATESr

Declared vector of coordinates of "best" edge detected in camera coordinate system.

Definition at line 127 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VEDGECOORDINATESwenu

Declared vector of coordinates of "best" edge detected in NED world coordinate system.

Definition at line 131 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VEDGECOORDINATESwned

Declared vector of coordinates of "best" edge detected in robot coordinate system.

Definition at line 129 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VGOALc

Declared vector of coordinates of goal in robot coordinate system.

Definition at line 124 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VGOALr

Declared vector of coordinates of goal in NED world coordinate system.

Definition at line 123 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VGOALwenu

Declared vector of coordinates of start position in NED world coordinate system.

Definition at line 121 of file wedgebug.c.

Referenced by wedgebug_init().

struct FloatVect3 VGOALwned

Declared vector of coordinates of goal in ENU world coordinate system.

Definition at line 122 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatVect3 VHOLDINGPOINTwned

Declared a vector to hold the current position, which is needed for calculating the distance traveled.

Definition at line 135 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VNEDwenu

Definition at line 193 of file wedgebug.c.

Referenced by wedgebug_init(), and wedgebug_periodic().

struct FloatVect3 VPBESTEDGECOORDINATESwned

Declared a vector to hold the position of a holding point (offten used to make sure drone stays still before stuff happens)

Definition at line 137 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VRwned

Definition at line 196 of file wedgebug.c.

Referenced by wedgebug_periodic().

struct FloatVect3 VSTARTwenu

Height of kernel for the median kernel.

Definition at line 117 of file wedgebug.c.

Referenced by wedgebug_init().

struct FloatVect3 VSTARTwned

Declared vector of coordinates of start position in ENU world coordinate system.

Definition at line 119 of file wedgebug.c.

Referenced by wedgebug_init().