Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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)
 
static struct image_tcopy_left_img_func (struct image_t *img, uint8_t camera_id)
 
static struct image_tcopy_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

Enumerator
DIRECT_CONTROL 
AUTONOMOUS_NAV 
AUTONOMOUS_GUIDED 

Definition at line 220 of file wedgebug.c.

◆ 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

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

◆ c_img_cropped

struct point_t c_img_cropped

Principal point of normal camera images.

Definition at line 184 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 226 of file wedgebug.c.

◆ current_state

enum navigation_state current_state

Definition at line 217 of file wedgebug.c.

◆ 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 187 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 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(), calculate_new_heading(), check_and_compute_data(), check_line(), colorfilter_func(), compensate_temperature(), compute_ff_vel(), dc_send_shot_position(), delayed_first_order_lowpass_initialize(), delayed_first_order_lowpass_propagate(), delayed_first_order_lowpass_set_delay(), delayed_first_order_lowpass_set_omega(), detect_gate_func(), detect_gate_init(), Dhane_distortion(), discrete_ekf_new(), distance_to_wall(), Drift_correction(), ecef_of_lla_d(), ecef_of_lla_f(), electrical_periodic(), error_output(), esc32_get_float(), esc32_put_float(), extract_information_from_parameters(), ezcurrent_read_event(), 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(), mav_exercise_periodic(), 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(), normal_random_gen(), 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(), sign(), sonar_vl53l1x_publish(), sonar_vl53l1x_read(), srf08_event(), stabilization_attitude_read_rc_roll_pitch_earth_quat_f(), stabilization_opticflow_vel_cb(), store_min_dist(), tfmini_i2c_periodic(), 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(), viewing_angle(), wgs84_ellipsoid_to_geoid_f(), wgs84_ellipsoid_to_geoid_i(), wind_estimator_periodic(), 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 96 of file wedgebug.c.

◆ img_cropped_info

struct crop_t img_cropped_info

Definition at line 92 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 80 of file wedgebug.c.

◆ img_dims

struct img_size_t img_dims

Definition at line 95 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 84 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 79 of file wedgebug.c.

◆ img_edges_int8_cropped

struct image_t img_edges_int8_cropped

Definition at line 82 of file wedgebug.c.

◆ img_left

struct image_t img_left

Definition at line 74 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 76 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 77 of file wedgebug.c.

◆ img_middle_int16_cropped

struct image_t img_middle_int16_cropped

Definition at line 85 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 81 of file wedgebug.c.

◆ img_right

struct image_t img_right

Image obtained from left camera (UYVY format)

Definition at line 75 of file wedgebug.c.

Referenced by SBM_OCV().

◆ img_right_int8

struct image_t img_right_int8

Definition at line 78 of file wedgebug.c.

◆ initial_heading

struct ES_angles initial_heading

Definition at line 239 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 98 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 102 of file wedgebug.c.

◆ median_kernel16bit

struct kernel_C1 median_kernel16bit

Definition at line 103 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 198 of file wedgebug.c.

◆ Rrwned

struct FloatRMat Rrwned

Definition at line 195 of file wedgebug.c.

◆ Rwnedwenu

struct FloatRMat Rwnedwenu

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

Definition at line 192 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 199 of file wedgebug.c.

◆ VDISTANCEPOSITIONwned

struct FloatVect3 VDISTANCEPOSITIONwned

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

Definition at line 133 of file wedgebug.c.

◆ VEDGECOORDINATESc

struct FloatVect3 VEDGECOORDINATESc

Declared vector of coordinates of goal in camera coordinate system.

Definition at line 125 of file wedgebug.c.

◆ VEDGECOORDINATESr

struct FloatVect3 VEDGECOORDINATESr

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

Definition at line 127 of file wedgebug.c.

◆ VEDGECOORDINATESwenu

struct FloatVect3 VEDGECOORDINATESwenu

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

Definition at line 131 of file wedgebug.c.

◆ VEDGECOORDINATESwned

struct FloatVect3 VEDGECOORDINATESwned

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

Definition at line 129 of file wedgebug.c.

◆ VGOALc

struct FloatVect3 VGOALc

Declared vector of coordinates of goal in robot coordinate system.

Definition at line 124 of file wedgebug.c.

◆ VGOALr

struct FloatVect3 VGOALr

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

Definition at line 123 of file wedgebug.c.

◆ VGOALwenu

struct FloatVect3 VGOALwenu

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

Definition at line 121 of file wedgebug.c.

◆ VGOALwned

struct FloatVect3 VGOALwned

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

Definition at line 122 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 135 of file wedgebug.c.

◆ VNEDwenu

struct FloatVect3 VNEDwenu

Definition at line 193 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 137 of file wedgebug.c.

◆ VRwned

struct FloatVect3 VRwned

Definition at line 196 of file wedgebug.c.

◆ VSTARTwenu

struct FloatVect3 VSTARTwenu

Height of kernel for the median kernel.

Definition at line 117 of file wedgebug.c.

◆ VSTARTwned

struct FloatVect3 VSTARTwned

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

Definition at line 119 of file wedgebug.c.