Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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

#define AVOIDANCE_AMOUNT_OF_BOARDS   6

Definition at line 45 of file obstacle_avoidance.c.

Referenced by serial_init(), and serial_update().

#define AVOIDANCE_HEIGHT_IN_MEASUREMENT_VALUES   6

Definition at line 49 of file obstacle_avoidance.c.

Referenced by serial_init().

#define AVOIDANCE_WIDTH_IN_MEASUREMENT_VALUES   6

Definition at line 53 of file obstacle_avoidance.c.

Referenced by serial_init().

#define AVOIDANCES_DISTANCES_HOR_COUNT   36

Definition at line 41 of file obstacle_avoidance.c.

Referenced by matrix_2_pingpong().

Function Documentation

void CN_calculate_target ( void  )

Definition at line 321 of file obstacle_avoidance.c.

References current_pos, dx_ref, dy_ref, heading_goal_f, 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:

void CN_escape_velocity ( void  )
void CN_matrix_butterworth ( void  )

Definition at line 295 of file obstacle_avoidance.c.

References butter_old, READimageBuffer_old, and size_matrix.

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.

void CN_potential_heading ( void  )
void CN_potential_velocity ( void  )
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:

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:

void serial_start ( void  )

Definition at line 159 of file obstacle_avoidance.c.

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

float A_kal = 1

Definition at line 83 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

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

Definition at line 108 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

struct FloatVect3 Attractforce_goal_send

Definition at line 136 of file obstacle_avoidance.c.

float b_damp = 0

Definition at line 110 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float B_kal = 0

Definition at line 84 of file obstacle_avoidance.c.

float c1_oa = 0.4

Definition at line 113 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float c2_oa = 0.4

Definition at line 114 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float c3_oa = 5.0

Definition at line 115 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float c4_oa = 0.5

Definition at line 116 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float c5_oa = 0.9

Definition at line 117 of file obstacle_avoidance.c.

float Cfreq = 1

Definition at line 101 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

struct NedCoor_f current_pos

Definition at line 77 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

uint8_t direction = 0

Definition at line 125 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

int8_t dis_treshold = 2

Definition at line 105 of file obstacle_avoidance.c.

float Dist_offset = 0

Definition at line 104 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

float dist_treshold = 1.25

Definition at line 66 of file obstacle_avoidance.c.

Referenced by serial_update().

float distances_hor[AVOIDANCES_DISTANCES_HOR_COUNT]

Definition at line 67 of file obstacle_avoidance.c.

Referenced by serial_update().

float dx_ref = 0

Definition at line 80 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

float dy_ref = 0

Definition at line 81 of file obstacle_avoidance.c.

Referenced by CN_calculate_target().

float epsilon = 0.0

Definition at line 119 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

uint8_t escape_flag = 0

Definition at line 128 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity().

float F1 = 0.1

Definition at line 99 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

float F2 = 0.9

Definition at line 100 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

struct FloatVect3 filter_repforce_old = {0.0, 0.0, 0.0}

Definition at line 94 of file obstacle_avoidance.c.

float H_kal = 1

Definition at line 85 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

uint8_t hysteris_flag

Definition at line 134 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

struct FloatVect2 init_target = {0, 0}

Definition at line 79 of file obstacle_avoidance.c.

float K_gain_send = 0

Definition at line 88 of file obstacle_avoidance.c.

float K_goal = 1

Definition at line 111 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float K_obst = 40

Definition at line 112 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float Kg = 100

Definition at line 103 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

float Ko = 60

Definition at line 102 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

float kv = 0.05

Definition at line 118 of file obstacle_avoidance.c.

Referenced by CN_potential_heading(), and CN_potential_velocity().

float new_heading_old

Definition at line 127 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

uint8_t obstacle_flag = 0

Definition at line 124 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

uint8_t point_index = 0

Definition at line 121 of file obstacle_avoidance.c.

float Q_kal = 0.05

Definition at line 86 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

float R_kal = 2

Definition at line 87 of file obstacle_avoidance.c.

Referenced by CN_matrix_Kalman_filter().

uint8_t* READimageBuffer_old

Definition at line 72 of file obstacle_avoidance.c.

Referenced by CN_matrix_butterworth().

float ref_yaw = 0.0

Definition at line 62 of file obstacle_avoidance.c.

float reference_pitch = 0.1

Definition at line 64 of file obstacle_avoidance.c.

Referenced by serial_update().

float reference_roll = 0.1

Definition at line 65 of file obstacle_avoidance.c.

Referenced by serial_update().

struct FloatVect3 Repulsionforce_Kan

Definition at line 107 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

struct FloatVect3 Repulsionforce_Kan_old = {0.0, 0.0, 0.0}

Definition at line 95 of file obstacle_avoidance.c.

struct FloatVect3 Repulsionforce_Kan_send

Definition at line 137 of file obstacle_avoidance.c.

struct FloatVect3 Repulsionforce_Used

Definition at line 106 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and CN_vector_velocity().

uint8_t set_bias = 0

Definition at line 123 of file obstacle_avoidance.c.

Referenced by CN_escape_velocity(), and CN_vector_escape_velocity().

float stereo_fow[2] = {1.0018, 0.7767}
float V_hys_high = 10

Definition at line 133 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

float V_hys_low = 5

Definition at line 132 of file obstacle_avoidance.c.

Referenced by CN_vector_velocity().

float v_max = 0.6

Definition at line 130 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and image_labeling().

float v_min = 0.3

Definition at line 129 of file obstacle_avoidance.c.

Referenced by CN_vector_escape_velocity(), and image_labeling().

float vmin = 0.2

Definition at line 120 of file obstacle_avoidance.c.

Referenced by CN_potential_velocity().

float waypoint_rot = 0