#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:

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)

static struct image_t *	copy_left_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)

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)

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

◆ ES_angles

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

◆ WEDGEBUG_CAMERA_LEFT_FPS

#define WEDGEBUG_CAMERA_LEFT_FPS 0

Definition at line 65 of file wedgebug.c.

◆ WEDGEBUG_CAMERA_RIGHT_FPS

#define WEDGEBUG_CAMERA_RIGHT_FPS 0

Definition at line 62 of file wedgebug.c.

Enumeration Type Documentation

◆ control_mode_state

enum control_mode_state

Enumerator
DIRECT_CONTROL
AUTONOMOUS_NAV
AUTONOMOUS_GUIDED

Definition at line 220 of file wedgebug.c.

◆ navigation_state

enum navigation_state

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

◆ are_setpoint_and_angle_reached()

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
	)

◆ background_processes()

void background_processes ( uint8_t save_images_flag )

◆ background_processes_16bit()

void background_processes_16bit ( uint8_t save_images_flag )

◆ copy_left_img_func()

static struct image_t* copy_left_img_func	(	struct image_t *	img,
		uint8_t	camera_id
	)

static

◆ copy_right_img_func()

static struct image_t* copy_right_img_func	(	struct image_t *	img,
		uint8_t	camera_id
	)

static

◆ depth_to_disp()

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

◆ disp_to_depth()

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

◆ disp_to_depth_16bit()

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

◆ disp_to_depth_img()

void disp_to_depth_img	(	struct image_t *	img8bit_input,
		struct image_t *	img16bit_output
	)

◆ dispfixed_to_disp()

float dispfixed_to_disp ( const int16_t d )

◆ find_best_edge_coordinates()

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
	)

◆ find_best_edge_coordinates2()

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
	)

◆ float_norm_two_angles()

float float_norm_two_angles	(	float	target_angle,
		float	current_angle
	)

◆ float_vect3_norm_two_points()

float float_vect3_norm_two_points	(	struct FloatVect3 *	V1,
		struct FloatVect3 *	V2
	)

◆ get_img_type()

const char * get_img_type ( enum image_type img_type )

Definition at line 357 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:

◆ getMedian16bit()

uint16_t getMedian16bit	(	uint16_t *	a,
		uint32_t	n
	)

◆ heading_towards_setpoint_WNED()

float heading_towards_setpoint_WNED ( struct FloatVect3 * VSETPOINTwned )

◆ heading_towards_waypoint()

float heading_towards_waypoint ( uint8_t wp )

◆ indx1d_a()

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

◆ indx1d_b()

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

◆ indx1d_c()

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

◆ is_heading_reached()

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

◆ is_setpoint_reached()

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

◆ maximum_intensity()

uint32_t maximum_intensity ( struct image_t * img )

◆ median_depth_to_point()

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

◆ median_disparity_to_point()

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

◆ principal_points()

void principal_points	(	struct point_t *	c_output,
		const struct point_t *	c_old_input,
		struct crop_t *	img_cropped_info
	)

◆ show_image_data()

void show_image_data ( struct image_t * img )

Definition at line 370 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:

◆ show_image_entry()

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

Definition at line 381 of file wedgebug.c.

References image_t::buf.

◆ show_rotation_matrix()

void show_rotation_matrix ( struct FloatRMat * R )

Definition at line 387 of file wedgebug.c.

References FloatRMat::m.

◆ thresholding_img()

void thresholding_img	(	struct image_t *	img,
		uint8_t	threshold
	)

◆ UYVYs_interlacing_H()

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

◆ UYVYs_interlacing_V()

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

◆ Va_to_Vb()

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

◆ Vb_to_Va()

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

◆ Vc_to_Vw()

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
	)

◆ Vi_to_Vc()

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
	)

◆ Vi_to_Vc16bit()

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
	)

◆ Vi_to_Vc_depth()

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
	)

◆ Vw_to_Vc()

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
	)

Variable Documentation

◆ allow_state_change_EDGE_SCAN

uint8_t allow_state_change_EDGE_SCAN

Definition at line 276 of file wedgebug.c.

◆ allow_state_change_MOVE_TO_EDGE

uint8_t allow_state_change_MOVE_TO_EDGE

Definition at line 274 of file wedgebug.c.

◆ allow_state_change_MOVE_TO_GOAL

uint8_t allow_state_change_MOVE_TO_GOAL

Definition at line 271 of file wedgebug.c.

◆ allow_state_change_POSITION_EDGE

uint8_t allow_state_change_POSITION_EDGE

Definition at line 275 of file wedgebug.c.

◆ allow_state_change_POSITION_GOAL

uint8_t allow_state_change_POSITION_GOAL

Definition at line 272 of file wedgebug.c.

◆ allow_state_change_WEDGEBUG_START

uint8_t allow_state_change_WEDGEBUG_START

Definition at line 273 of file wedgebug.c.

◆ b

float b

Initial value:

= WEDGEBUG_CAMERA_BASELINE /

1000.00

Definition at line 202 of file wedgebug.c.

Referenced by calc_ntc(), cmp_array(), cmp_flow(), cmp_i(), cmpfunc(), cmpTimeUs(), CrcUpdate(), dim_mod(), dqrls(), dqrlss(), dqrsl(), ekf_set_diag(), err_nd(), err_sum_nd(), float_from_buf(), float_mat_combine(), float_mat_copy(), float_mat_diff(), float_mat_mul(), float_mat_mul_copy(), float_mat_mul_transpose(), float_mat_sum(), float_mat_sum_scaled(), float_mat_vect_mul(), float_vect_add(), float_vect_copy(), float_vect_diff(), float_vect_dot_product(), float_vect_mul(), float_vect_sub(), float_vect_sum(), get_two_intersects(), gvf_ellipse_info(), gvf_ellipse_wp(), gvf_ellipse_XY(), gvf_get_two_intersects(), gvf_line(), gvf_line_info(), gvf_line_wp_heading(), gvf_line_XY_heading(), gvf_sin_info(), gvf_sin_XY_alpha(), image_switch(), init_fourth_order_high_pass(), int32_gcd(), int32_mat_mul(), int32_vect_add(), int32_vect_copy(), int32_vect_diff(), int32_vect_mul(), int32_vect_sub(), int32_vect_sum(), light_ws2812_arch_set(), lla_of_ecef_d(), lla_of_ecef_f(), main(), Matrix_Multiply(), nav_goto_block(), notch_filter_update(), parse_mf_daq_msg(), pprz_mode_update(), pprz_qr_float(), pprz_svd_solve_float(), px4flash_event(), pythag(), qr_solve(), qr_solve_wrapper(), quat_from_earth_cmd_f(), r8mat_l_solve(), r8mat_lt_solve(), set_nav_block(), syslink_compute_cksum(), syslink_put_byte(), uint16_from_buf(), and wls_alloc().

◆ block_size_disparities

int block_size_disparities = 25

Number of disparity levels (0-this number)

Definition at line 256 of file wedgebug.c.

◆ c_img

struct point_t c_img

Definition at line 180 of file wedgebug.c.

◆ c_img_cropped

struct point_t c_img_cropped

Principal point of normal camera images.

Definition at line 180 of file wedgebug.c.

◆ clock_background_processes

clock_t clock_background_processes

Definition at line 249 of file wedgebug.c.

◆ clock_FSM

clock_t clock_FSM

Definition at line 250 of file wedgebug.c.

◆ clock_total_time

clock_t clock_total_time

Definition at line 246 of file wedgebug.c.

◆ clock_total_time_current

clock_t clock_total_time_current

Definition at line 247 of file wedgebug.c.

◆ clock_total_time_previous

clock_t clock_total_time_previous

Definition at line 248 of file wedgebug.c.

◆ counter_cycles

double counter_cycles

Definition at line 245 of file wedgebug.c.

◆ counter_state

double counter_state[NUMBER_OF_STATES]

Definition at line 244 of file wedgebug.c.

◆ current_mode

enum control_mode_state current_mode

Definition at line 204 of file wedgebug.c.

◆ current_state

enum navigation_state current_state

Definition at line 204 of file wedgebug.c.

Referenced by sts3032_event().

◆ distance_robot_edge_goal

float distance_robot_edge_goal

Definition at line 265 of file wedgebug.c.

◆ distance_traveled

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.

◆ edge_found_macro_confidence

int16_t edge_found_macro_confidence

This is the confidence that the desired heading is reached.

Definition at line 159 of file wedgebug.c.

◆ edge_search_area

struct crop_t edge_search_area

Principal point of cropped camera images.

Definition at line 180 of file wedgebug.c.

◆ f

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 delayed_first_order_lowpass_initialize(), delayed_first_order_lowpass_propagate(), delayed_first_order_lowpass_set_delay(), delayed_first_order_lowpass_set_omega(), ecef_of_lla_d(), ecef_of_lla_f(), eff_scheduling_falcon_report(), esc32_get_float(), esc32_put_float(), float_from_buf(), gc_of_gd_lat_d(), image_labeling(), int32_vect2_normalize(), lla_of_ecef_d(), lla_of_ecef_f(), runge_kutta_1_float(), runge_kutta_2_float(), runge_kutta_4_float(), srf08_event(), write_exif_jpeg(), xgeqrf(), and xgeqrf_f().

◆ free_path_confidence

int16_t free_path_confidence

This is the confidence that an obstacle was spotted.

Definition at line 154 of file wedgebug.c.

◆ heading

◆ heading_confidence

int16_t heading_confidence

This is the confidence that the desired position was reached.

Definition at line 156 of file wedgebug.c.

◆ heat_map_type

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.

◆ img_cropped_dims

struct img_size_t img_cropped_dims

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

Definition at line 1 of file wedgebug.c.

◆ img_cropped_info

struct crop_t img_cropped_info

Definition at line 1 of file wedgebug.c.

Referenced by SBM_OCV().

◆ img_depth_int16_cropped

struct image_t img_depth_int16_cropped

Image obtained after simple block matching.

Definition at line 1 of file wedgebug.c.

◆ img_dims

struct img_size_t img_dims

Definition at line 1 of file wedgebug.c.

◆ img_disparity_int16_cropped

struct image_t img_disparity_int16_cropped

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

Definition at line 1 of file wedgebug.c.

◆ img_disparity_int8_cropped

struct image_t img_disparity_int8_cropped

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

Definition at line 1 of file wedgebug.c.

◆ img_edges_int8_cropped

struct image_t img_edges_int8_cropped

Definition at line 1 of file wedgebug.c.

◆ img_left

struct image_t img_left

Definition at line 1 of file wedgebug.c.

Referenced by SBM_OCV().

◆ img_left_int8

struct image_t img_left_int8

Image obtained from right camera (UYVY format)

Definition at line 1 of file wedgebug.c.

◆ img_left_int8_cropped

struct image_t img_left_int8_cropped

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

Definition at line 1 of file wedgebug.c.

◆ img_middle_int16_cropped

struct image_t img_middle_int16_cropped

Definition at line 1 of file wedgebug.c.

◆ img_middle_int8_cropped

struct image_t img_middle_int8_cropped

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

Definition at line 1 of file wedgebug.c.

◆ img_right

struct image_t img_right

Image obtained from left camera (UYVY format)

Definition at line 1 of file wedgebug.c.

Referenced by SBM_OCV().

◆ img_right_int8

struct image_t img_right_int8

Definition at line 1 of file wedgebug.c.

◆ initial_heading

struct ES_angles initial_heading

Definition at line 204 of file wedgebug.c.

◆ is_edge_found_macro_flag

uint8_t is_edge_found_macro_flag

Set to 1 if heading is reached, 0 otherwise.

Definition at line 175 of file wedgebug.c.

◆ is_edge_found_micro_flag

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.

◆ is_heading_reached_flag

uint8_t is_heading_reached_flag

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

Definition at line 174 of file wedgebug.c.

◆ is_mode_changed_flag

uint8_t is_mode_changed_flag

Set to 1 if state was changed, 0 otherwise.

Definition at line 179 of file wedgebug.c.

◆ is_no_edge_found_flag

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.

◆ is_obstacle_detected_flag

uint8_t is_obstacle_detected_flag

Set to 1 if setpoint is reached, 0 otherwise.

Definition at line 172 of file wedgebug.c.

◆ is_path_free_flag

uint8_t is_path_free_flag

Set to 1 if obstacle is detected, 0 otherwise.

Definition at line 173 of file wedgebug.c.

◆ is_setpoint_reached_flag

uint8_t is_setpoint_reached_flag

Definition at line 171 of file wedgebug.c.

◆ is_start_reached_flag

uint8_t is_start_reached_flag

This is the max confidence that no edges were found.

Definition at line 170 of file wedgebug.c.

◆ is_state_changed_flag

uint8_t is_state_changed_flag

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

Definition at line 178 of file wedgebug.c.

◆ is_total_timer_on_flag

uint8_t is_total_timer_on_flag

Definition at line 279 of file wedgebug.c.

◆ K_median_h

uint16_t K_median_h

Width of kernel for the median kernel.

Definition at line 113 of file wedgebug.c.

◆ K_median_w

uint16_t K_median_w

SE size for the Sobel operation, to detect edges.

Definition at line 112 of file wedgebug.c.

◆ kernel_median_dims

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 1 of file wedgebug.c.

◆ max_edge_found_macro_confidence

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.

◆ max_edge_found_micro_confidence

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.

◆ max_edge_search_angle

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.

◆ max_free_path_confidence

int16_t max_free_path_confidence

This is the max confidence that an obstacle was spotted.

Definition at line 162 of file wedgebug.c.

◆ max_heading_confidence

int16_t max_heading_confidence

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

Definition at line 164 of file wedgebug.c.

◆ max_no_edge_found_confidence

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.

◆ max_obstacle_confidence

int16_t max_obstacle_confidence

This is the confidence that no edge was found.

Definition at line 161 of file wedgebug.c.

◆ max_position_confidence

int16_t max_position_confidence

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

Definition at line 163 of file wedgebug.c.

◆ median_depth_in_front

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.

◆ median_disparity_in_front

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.

◆ median_kernel

struct kernel_C1 median_kernel

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

Definition at line 1 of file wedgebug.c.

◆ median_kernel16bit

struct kernel_C1 median_kernel16bit

Definition at line 1 of file wedgebug.c.

◆ min_disparity

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

◆ N_disparities

int N_disparities = 64

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

Definition at line 255 of file wedgebug.c.

◆ no_edge_found_confidence

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.

◆ number_of_states

uint8_t number_of_states

Definition at line 264 of file wedgebug.c.

◆ obstacle_confidence

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.

◆ position_confidence

int16_t position_confidence

This is the confidence that no obstacle was spotted.

Definition at line 155 of file wedgebug.c.

◆ previous_mode

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.

◆ previous_state

uint8_t previous_state

Definition at line 253 of file wedgebug.c.

◆ Rcr

struct FloatRMat Rcr

Definition at line 180 of file wedgebug.c.

◆ Rrwned

struct FloatRMat Rrwned

Definition at line 180 of file wedgebug.c.

◆ Rwnedwenu

struct FloatRMat Rwnedwenu

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

Definition at line 180 of file wedgebug.c.

◆ save_images_flag

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.

◆ SE_closing_OCV

int SE_closing_OCV

SE size for the opening operation.

Definition at line 107 of file wedgebug.c.

◆ SE_dilation_OCV_1

int SE_dilation_OCV_1

SE size for the closing operation.

Definition at line 108 of file wedgebug.c.

◆ SE_dilation_OCV_2

int SE_dilation_OCV_2

SE size for the first dilation operation.

Definition at line 109 of file wedgebug.c.

◆ SE_erosion_OCV

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.

◆ SE_opening_OCV

int SE_opening_OCV

Definition at line 106 of file wedgebug.c.

◆ SE_sobel_OCV

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.

◆ threshold_depth_of_edges

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.

◆ threshold_disparity_of_edges

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.

◆ threshold_distance_to_angle

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.

◆ threshold_distance_to_goal

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.

◆ threshold_distance_to_goal_direct

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.

◆ threshold_distance_to_goal_manual

float threshold_distance_to_goal_manual

Definition at line 280 of file wedgebug.c.

◆ threshold_edge_magnitude

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.

◆ threshold_median_depth

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.

◆ threshold_median_disparity

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.

◆ time_state

double time_state[NUMBER_OF_STATES]

Definition at line 243 of file wedgebug.c.

◆ VCr

struct FloatVect3 VCr

Definition at line 180 of file wedgebug.c.

◆ VDISTANCEPOSITIONwned

struct FloatVect3 VDISTANCEPOSITIONwned

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

Definition at line 113 of file wedgebug.c.

◆ VEDGECOORDINATESc

struct FloatVect3 VEDGECOORDINATESc

Declared vector of coordinates of goal in camera coordinate system.

Definition at line 113 of file wedgebug.c.

◆ VEDGECOORDINATESr

struct FloatVect3 VEDGECOORDINATESr

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

Definition at line 113 of file wedgebug.c.

◆ VEDGECOORDINATESwenu

struct FloatVect3 VEDGECOORDINATESwenu

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

Definition at line 113 of file wedgebug.c.

◆ VEDGECOORDINATESwned

struct FloatVect3 VEDGECOORDINATESwned

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

Definition at line 113 of file wedgebug.c.

◆ VGOALc

struct FloatVect3 VGOALc

Declared vector of coordinates of goal in robot coordinate system.

Definition at line 113 of file wedgebug.c.

◆ VGOALr

struct FloatVect3 VGOALr

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

Definition at line 113 of file wedgebug.c.

◆ VGOALwenu

struct FloatVect3 VGOALwenu

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

Definition at line 113 of file wedgebug.c.

◆ VGOALwned

struct FloatVect3 VGOALwned

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

Definition at line 113 of file wedgebug.c.

◆ VHOLDINGPOINTwned

struct FloatVect3 VHOLDINGPOINTwned

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

Definition at line 113 of file wedgebug.c.

◆ VNEDwenu

struct FloatVect3 VNEDwenu

Definition at line 180 of file wedgebug.c.

◆ VPBESTEDGECOORDINATESwned

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 113 of file wedgebug.c.

◆ VRwned

struct FloatVect3 VRwned

Definition at line 180 of file wedgebug.c.

◆ VSTARTwenu

struct FloatVect3 VSTARTwenu

Height of kernel for the median kernel.

Definition at line 113 of file wedgebug.c.

◆ VSTARTwned

struct FloatVect3 VSTARTwned

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

Definition at line 113 of file wedgebug.c.

Data Structures

Macros

Enumerations

Functions

Variables

Data Structure Documentation

◆ ES_angles

Macro Definition Documentation

◆ WEDGEBUG_CAMERA_LEFT_FPS

◆ WEDGEBUG_CAMERA_RIGHT_FPS

Enumeration Type Documentation

◆ control_mode_state

◆ navigation_state

Function Documentation

◆ are_setpoint_and_angle_reached()

◆ background_processes()

◆ background_processes_16bit()

◆ copy_left_img_func()

◆ copy_right_img_func()

◆ depth_to_disp()

◆ disp_to_depth()

◆ disp_to_depth_16bit()

◆ disp_to_depth_img()

◆ dispfixed_to_disp()

◆ find_best_edge_coordinates()

◆ find_best_edge_coordinates2()

◆ float_norm_two_angles()

◆ float_vect3_norm_two_points()

◆ get_img_type()

◆ getMedian16bit()

◆ heading_towards_setpoint_WNED()

◆ heading_towards_waypoint()

◆ indx1d_a()

◆ indx1d_b()

◆ indx1d_c()

◆ is_heading_reached()

◆ is_setpoint_reached()

◆ maximum_intensity()

◆ median_depth_to_point()

◆ median_disparity_to_point()

◆ principal_points()

◆ show_image_data()

◆ show_image_entry()

◆ show_rotation_matrix()

◆ thresholding_img()

◆ UYVYs_interlacing_H()

◆ UYVYs_interlacing_V()

◆ Va_to_Vb()

◆ Vb_to_Va()

◆ Vc_to_Vw()

◆ Vi_to_Vc()

◆ Vi_to_Vc16bit()

◆ Vi_to_Vc_depth()

◆ Vw_to_Vc()

Variable Documentation

◆ 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

◆ b

◆ block_size_disparities

◆ c_img

◆ 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

◆ distance_robot_edge_goal

◆ distance_traveled

◆ edge_found_macro_confidence

◆ edge_search_area

◆ f

◆ free_path_confidence