Obstacle avoidance methods. More...

#include <stdio.h>
#include <sys/fcntl.h>
#include <math.h>
#include <unistd.h>
#include <inttypes.h>
#include "state.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/datalink/telemetry.h"
#include "modules/stereocam/stereocam.h"
#include "modules/obstacle_avoidance/guidance_OA.h"
#include "modules/obstacle_avoidance/obstacle_avoidance.h"

Include dependency graph for obstacle_avoidance.c:

Go to the source code of this file.

Macros
#define	AVOIDANCES_DISTANCES_HOR_COUNT 36

#define	AVOIDANCE_AMOUNT_OF_BOARDS 6

#define	AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES 6

#define	AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES 6

Functions
void	serial_init (void)

void	serial_start (void)

void	setAnglesMeasurements (float anglesMeasurements, float centersensorRad, float fieldOfViewRad, uint16_t size_matrix_local)

void	serial_update (void)

void	matrix_2_pingpong (float distancesMeters, uint16_t size_matrix_local, float *distances_hor_local)

void	CN_matrix_Kalman_filter (void)

void	CN_matrix_butterworth (void)

void	CN_calculate_target (void)

void	pingpong_euler (float distances_hor_local, float horizontalAnglesMeasurements, int horizontalAmountOfMeasurements, float attitude_reference_pitch, float attitude_reference_roll, float dist_treshold_local)

void	CN_potential_heading (void)

void	CN_potential_velocity (void)

void	CN_vector_velocity (void)

void	CN_vector_escape_velocity (void)

void	CN_escape_velocity (void)

Variables
uint16_t	size_matrix [] = {AVOIDANCE_AMOUNT_OF_BOARDS, AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES, AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES}

float	stereo_fow [2] = {1.0018, 0.7767}

float	angle_hor_board [] = {0, 1.0472, 2.0944, 3.1416, -2.0944, -1.0472}

int16_t	focal = 118 * 6

float	baseline = 60

float	ref_yaw = 0.0

float	reference_pitch = 0.1

float	reference_roll = 0.1

float	dist_treshold = 1.25

float	distances_hor [AVOIDANCES_DISTANCES_HOR_COUNT]

uint8_t *	READimageBuffer_old

float	heading_goal_f = 0

float	heading_goal_ref

struct FloatVect2	pos_diff

struct NedCoor_f	current_pos

struct FloatVect2	target

struct FloatVect2	init_target = {0, 0}

float	dx_ref = 0

float	dy_ref = 0

float	A_kal = 1

float	B_kal = 0

float	H_kal = 1

float	Q_kal = 0.05

float	R_kal = 2

float	K_gain_send = 0

float	Pest_new [AVOIDANCE_AMOUNT_OF_BOARDS AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]

float	Xest_new [AVOIDANCE_AMOUNT_OF_BOARDS AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]

struct FloatVect3	filter_repforce_old = {0.0, 0.0, 0.0}

struct FloatVect3	Repulsionforce_Kan_old = {0.0, 0.0, 0.0}

float	butter_old [AVOIDANCE_AMOUNT_OF_BOARDS AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES]

float	F1 = 0.1

float	F2 = 0.9

float	Cfreq = 1

float	Ko = 60

float	Kg = 100

float	Dist_offset = 0

int8_t	dis_treshold = 2

struct FloatVect3	Repulsionforce_Used

struct FloatVect3	Repulsionforce_Kan

struct FloatVect3	Attractforce_goal

float	b_damp = 0

float	K_goal = 1

float	K_obst = 40

float	c1_oa = 0.4

float	c2_oa = 0.4

float	c3_oa = 5.0

float	c4_oa = 0.5

float	c5_oa = 0.9

float	kv = 0.05

float	epsilon = 0.0

float	vmin = 0.2

uint8_t	point_index = 0

uint8_t	set_bias = 0

uint8_t	obstacle_flag = 0

uint8_t	direction = 0

float	waypoint_rot = 0

float	new_heading_old

uint8_t	escape_flag = 0

float	v_min = 0.3

float	v_max = 0.6

float	V_hys_low = 5

float	V_hys_high = 10

uint8_t	hysteris_flag

struct FloatVect3	Attractforce_goal_send

struct FloatVect3	Repulsionforce_Kan_send

Detailed Description

Obstacle avoidance methods.

Definition in file obstacle_avoidance.c.

Macro Definition Documentation

◆ AVOIDANCE_AMOUNT_OF_BOARDS

#define AVOIDANCE_AMOUNT_OF_BOARDS 6

Definition at line 45 of file obstacle_avoidance.c.

◆ AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES

#define AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES 6

Definition at line 49 of file obstacle_avoidance.c.

◆ AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES

#define AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES 6

Definition at line 53 of file obstacle_avoidance.c.

◆ AVOIDANCES_DISTANCES_HOR_COUNT

#define AVOIDANCES_DISTANCES_HOR_COUNT 36

Definition at line 41 of file obstacle_avoidance.c.

Function Documentation

◆ CN_calculate_target()

void CN_calculate_target ( void )

Definition at line 321 of file obstacle_avoidance.c.

References current_pos, dx_ref, dy_ref, heading_goal_f, init_target, OA_method_flag, pos_diff, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), target, waypoint_rot, FloatVect2::x, NedCoor_f::x, FloatVect2::y, and NedCoor_f::y.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CN_escape_velocity()

void CN_escape_velocity ( void )

Definition at line 1032 of file obstacle_avoidance.c.

References angle_hor_board, baseline, direction, FLOAT_ANGLE_NORMALIZE, focal, heading_goal_f, heading_goal_ref, min_disparity, new_heading, new_heading_old, obstacle_flag, pos_diff, FloatEulers::psi, ref_pitch, ref_roll, set_bias, size_matrix, speed_pot, stateGetNedToBodyEulers_f(), stereo_fow, VECT2_NORM2, vref_max, and waypoint_rot.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CN_matrix_butterworth()

void CN_matrix_butterworth ( void )

Definition at line 295 of file obstacle_avoidance.c.

References butter_old, READimageBuffer_old, and size_matrix.

◆ CN_matrix_Kalman_filter()

void CN_matrix_Kalman_filter ( void )

Definition at line 263 of file obstacle_avoidance.c.

References A_kal, H_kal, Pest_new, Q_kal, R_kal, size_matrix, and Xest_new.

◆ CN_potential_heading()

void CN_potential_heading ( void )

Definition at line 404 of file obstacle_avoidance.c.

References angle_hor_board, b_damp, baseline, c1_oa, c2_oa, c3_oa, c4_oa, epsilon, FLOAT_ANGLE_NORMALIZE, focal, heading_goal_f, heading_goal_ref, K_goal, K_obst, kv, min_disparity, pos_diff, FloatEulers::psi, FloatRates::r, r_dot_new, size_matrix, speed_pot, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stereo_fow, VECT2_NORM2, and vref_max.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CN_potential_velocity()

void CN_potential_velocity ( void )

Definition at line 506 of file obstacle_avoidance.c.

References alpha_fil, angle_hor_board, b_damp, baseline, c1_oa, c2_oa, c3_oa, c4_oa, epsilon, FLOAT_ANGLE_NORMALIZE, focal, heading_goal_f, heading_goal_ref, K_goal, K_obst, kv, min_disparity, new_heading, pos_diff, FloatEulers::psi, FloatRates::r, r_dot_new, ref_pitch, size_matrix, speed_pot, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stereo_fow, VECT2_NORM2, vmin, and vref_max.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CN_vector_escape_velocity()

void CN_vector_escape_velocity ( void )

Definition at line 777 of file obstacle_avoidance.c.

References angle_hor_board, Attractforce_goal, baseline, Cfreq, direction, Dist_offset, escape_flag, F1, F2, filter_repforce_old, FLOAT_ANGLE_NORMALIZE, focal, heading_goal_f, heading_goal_ref, Kg, Ko, min_disparity, new_heading, new_heading_old, obstacle_flag, pos_diff, FloatEulers::psi, ref_pitch, ref_roll, repulsionforce_filter_flag, Repulsionforce_Kan, Repulsionforce_Kan_old, Repulsionforce_Used, set_bias, size_matrix, speed_pot, stateGetNedToBodyEulers_f(), stereo_fow, v_max, v_min, VECT2_NORM2, VECT3_ADD, VECT3_COPY, VECT3_SMUL, vref_max, waypoint_rot, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CN_vector_velocity()

void CN_vector_velocity ( void )

Definition at line 590 of file obstacle_avoidance.c.

References angle_hor_board, Attractforce_goal, baseline, Cfreq, Dist_offset, F1, F2, filter_repforce_old, focal, heading_goal_f, heading_goal_ref, hysteris_flag, Kg, Ko, min_disparity, pos_diff, FloatEulers::psi, ref_pitch, ref_roll, repulsionforce_filter_flag, Repulsionforce_Kan, Repulsionforce_Kan_old, Repulsionforce_Used, size_matrix, stateGetNedToBodyEulers_f(), stereo_fow, V_hys_high, V_hys_low, VECT2_NORM2, VECT3_ADD, VECT3_COPY, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by serial_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ matrix_2_pingpong()

void matrix_2_pingpong	(	float *	distancesMeters,
		uint16_t *	size_matrix_local,
		float *	distances_hor_local
	)

Definition at line 237 of file obstacle_avoidance.c.

References AVOIDANCES_DISTANCES_HOR_COUNT.

Referenced by serial_update().

Here is the caller graph for this function:

◆ pingpong_euler()

void pingpong_euler	(	float *	distances_hor_local,
		float *	horizontalAnglesMeasurements,
		int	horizontalAmountOfMeasurements,
		float	attitude_reference_pitch,
		float	attitude_reference_roll,
		float	dist_treshold_local
	)