Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
obstacle_avoidance.c File Reference

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_tREADimageBuffer_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  )

◆ 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  )

◆ CN_potential_velocity()

void CN_potential_velocity ( void  )

◆ CN_vector_escape_velocity()

◆ CN_vector_velocity()

◆ 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 
)

Definition at line 357 of file obstacle_avoidance.c.

References ref_pitch, and ref_roll.

Referenced by serial_update().

+ Here is the caller graph for this function:

◆ serial_init()

◆ serial_start()

void serial_start ( void  )

Definition at line 159 of file obstacle_avoidance.c.

◆ serial_update()

◆ setAnglesMeasurements()

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

Definition at line 164 of file obstacle_avoidance.c.

Referenced by serial_update().

+ Here is the caller graph for this function:

Variable Documentation

◆ A_kal

float A_kal = 1

Definition at line 83 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

◆ angle_hor_board

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

◆ Attractforce_goal

struct FloatVect3 Attractforce_goal

Definition at line 108 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ Attractforce_goal_send

struct FloatVect3 Attractforce_goal_send

Definition at line 136 of file obstacle_avoidance.c.

◆ b_damp

float b_damp = 0

Definition at line 110 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ B_kal

float B_kal = 0

Definition at line 84 of file obstacle_avoidance.c.

◆ baseline

◆ butter_old

◆ c1_oa

float c1_oa = 0.4

Definition at line 113 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ c2_oa

float c2_oa = 0.4

Definition at line 114 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ c3_oa

float c3_oa = 5.0

Definition at line 115 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ c4_oa

float c4_oa = 0.5

Definition at line 116 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ c5_oa

float c5_oa = 0.9

Definition at line 117 of file obstacle_avoidance.c.

◆ Cfreq

float Cfreq = 1

Definition at line 101 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ current_pos

struct NedCoor_f current_pos

Definition at line 77 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

◆ direction

uint8_t direction = 0

◆ dis_treshold

int8_t dis_treshold = 2

Definition at line 105 of file obstacle_avoidance.c.

◆ Dist_offset

float Dist_offset = 0

Definition at line 104 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ dist_treshold

float dist_treshold = 1.25

Definition at line 66 of file obstacle_avoidance.c.

Referenced by serial_update().

◆ distances_hor

float distances_hor[AVOIDANCES_DISTANCES_HOR_COUNT]

Definition at line 67 of file obstacle_avoidance.c.

Referenced by serial_update().

◆ dx_ref

float dx_ref = 0

Definition at line 80 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

◆ dy_ref

float dy_ref = 0

Definition at line 81 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

◆ epsilon

◆ escape_flag

uint8_t escape_flag = 0

Definition at line 128 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity().

◆ F1

float F1 = 0.1

Definition at line 99 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ F2

float F2 = 0.9

Definition at line 100 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ filter_repforce_old

struct FloatVect3 filter_repforce_old = {0.0, 0.0, 0.0}

Definition at line 94 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ focal

◆ H_kal

float H_kal = 1

Definition at line 85 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

◆ heading_goal_f

◆ heading_goal_ref

◆ hysteris_flag

uint8_t hysteris_flag

Definition at line 134 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

◆ init_target

struct FloatVect2 init_target = {0, 0}

Definition at line 79 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

◆ K_gain_send

float K_gain_send = 0

Definition at line 88 of file obstacle_avoidance.c.

◆ K_goal

float K_goal = 1

Definition at line 111 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ K_obst

float K_obst = 40

Definition at line 112 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ Kg

float Kg = 100

Definition at line 103 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ Ko

float Ko = 60

Definition at line 102 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ kv

float kv = 0.05

Definition at line 118 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

◆ new_heading_old

float new_heading_old

Definition at line 127 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

◆ obstacle_flag

uint8_t obstacle_flag = 0

Definition at line 124 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

◆ Pest_new

◆ point_index

uint8_t point_index = 0

Definition at line 121 of file obstacle_avoidance.c.

◆ pos_diff

◆ Q_kal

float Q_kal = 0.05

Definition at line 86 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

◆ R_kal

float R_kal = 2

Definition at line 87 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

◆ READimageBuffer_old

uint8_t* READimageBuffer_old

Definition at line 72 of file obstacle_avoidance.c.

Referenced by CN_matrix_butterworth().

◆ ref_yaw

float ref_yaw = 0.0

Definition at line 62 of file obstacle_avoidance.c.

◆ reference_pitch

float reference_pitch = 0.1

Definition at line 64 of file obstacle_avoidance.c.

Referenced by serial_update().

◆ reference_roll

float reference_roll = 0.1

Definition at line 65 of file obstacle_avoidance.c.

Referenced by serial_update().

◆ Repulsionforce_Kan

struct FloatVect3 Repulsionforce_Kan

Definition at line 107 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ Repulsionforce_Kan_old

struct FloatVect3 Repulsionforce_Kan_old = {0.0, 0.0, 0.0}

Definition at line 95 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ Repulsionforce_Kan_send

struct FloatVect3 Repulsionforce_Kan_send

Definition at line 137 of file obstacle_avoidance.c.

◆ Repulsionforce_Used

struct FloatVect3 Repulsionforce_Used

Definition at line 106 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

◆ set_bias

uint8_t set_bias = 0

Definition at line 123 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

◆ size_matrix

◆ stereo_fow

float stereo_fow[2] = {1.0018, 0.7767}

◆ target

◆ V_hys_high

float V_hys_high = 10

Definition at line 133 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

◆ V_hys_low

float V_hys_low = 5

Definition at line 132 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

◆ v_max

float v_max = 0.6

Definition at line 130 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and image_labeling().

◆ v_min

float v_min = 0.3

Definition at line 129 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and image_labeling().

◆ vmin

float vmin = 0.2

Definition at line 120 of file obstacle_avoidance.c.

Referenced by CN_potential_velocity().

◆ waypoint_rot

float waypoint_rot = 0

◆ Xest_new