|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
functions to create a grid map More...
#include "rover_obstacles.h"#include "math/pprz_geodetic_float.h"#include "modules/datalink/telemetry.h"#include "state.h"#include "firmwares/rover/navigation.h"#include "modules/core/abi.h"Go to the source code of this file.
Macros | |
| #define | P_FREE 0.4 |
| #define | P_OCC 0.7 |
| #define | L_MIN -127 |
| #define | L_MAX 127 |
| #define | L0 0 |
| #define | P_T 0.95 |
| #define | SCALE 20.0f |
| #define | OBSTACLES_RECEIVE_ID ABI_BROADCAST |
| #define | DECAY_INTERVAL 5000 |
| #define | DECAY 5 |
Functions | |
| static void | lidar_cb (uint8_t sender_id, uint32_t stamp, float distance, float angle) |
| static void | send_obstacle_grid (struct transport_tx *trans, struct link_device *dev) |
| static void | send_grid_init (struct transport_tx *trans, struct link_device *dev) |
| void | init_grid (uint8_t pa, uint8_t pb) |
| void | init_grid_4 (uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4) |
| void | obtain_cell_xy (float px, float py, int *cell_x, int *cell_y) |
| void | fill_cell (float px, float py) |
| void | fill_bayesian_cell (float px, float py) |
| void | fill_free_cells (float lidar, float angle) |
| void | decay_map () |
| void | update_line_bayes (int x0, int y0, int x1, int y1) |
| void | update_cell (int x, int y, int new_value) |
| void | compute_cell_bayes (int x, int y, bool is_occupied) |
| void | check_probs (void) |
| void | ins_update_lidar (float distance, float angle) |
Variables | |
| static uint32_t | last_s = 0 |
| static abi_event | lidar_ev |
| float | max_distance = 5 |
| static float | POCC = 0 |
| static float | PFREE = 0 |
| static float | PT = 0 |
| static int8_t | LT |
| static int8_t | LOCC |
| static int8_t | LFREE |
| world_grid | obstacle_grid |
| uint8_t | grid_block_size = GRID_BLOCK_SIZE |
functions to create a grid map
Definition in file rover_obstacles.c.
| #define DECAY 5 |
Definition at line 58 of file rover_obstacles.c.
Referenced by init_grid_4().
| #define DECAY_INTERVAL 5000 |
Definition at line 57 of file rover_obstacles.c.
Referenced by decay_map().
| #define L0 0 |
Definition at line 45 of file rover_obstacles.c.
Referenced by init_grid().
| #define L_MAX 127 |
Definition at line 44 of file rover_obstacles.c.
Referenced by compute_cell_bayes().
| #define L_MIN -127 |
Definition at line 43 of file rover_obstacles.c.
Referenced by compute_cell_bayes().
| #define OBSTACLES_RECEIVE_ID ABI_BROADCAST |
Definition at line 53 of file rover_obstacles.c.
Referenced by init_grid_4().
| #define P_FREE 0.4 |
Definition at line 41 of file rover_obstacles.c.
Referenced by init_grid_4().
| #define P_OCC 0.7 |
Definition at line 42 of file rover_obstacles.c.
Referenced by init_grid_4().
| #define P_T 0.95 |
Definition at line 46 of file rover_obstacles.c.
Referenced by init_grid_4().
| #define SCALE 20.0f |
Definition at line 48 of file rover_obstacles.c.
Referenced by check_probs().
| void check_probs | ( | void | ) |
Definition at line 344 of file rover_obstacles.c.
References LFREE, LOCC, LT, obstacle_grid, PFREE, POCC, PT, and SCALE.
Referenced by compute_cell_bayes().
| void compute_cell_bayes | ( | int | x, |
| int | y, | ||
| bool | is_occupied ) |
Definition at line 325 of file rover_obstacles.c.
References check_probs(), L_MAX, L_MIN, LFREE, LOCC, N_COL_GRID, N_ROW_GRID, obstacle_grid, update_cell(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by fill_bayesian_cell(), fill_free_cells(), and update_line_bayes().
| void decay_map | ( | void | ) |
Definition at line 256 of file rover_obstacles.c.
References DECAY_INTERVAL, get_sys_time_msec(), last_s, N_COL_GRID, N_ROW_GRID, obstacle_grid, update_cell(), EnuCoor_f::x, and EnuCoor_f::y.
| void fill_bayesian_cell | ( | float | px, |
| float | py ) |
Definition at line 203 of file rover_obstacles.c.
References compute_cell_bayes(), obstacle_grid, obtain_cell_xy(), rover_pos, stateGetPositionEnu_f(), and update_line_bayes().
Referenced by ins_update_lidar().
| void fill_cell | ( | float | px, |
| float | py ) |
Definition at line 194 of file rover_obstacles.c.
References obstacle_grid, and obtain_cell_xy().
| void fill_free_cells | ( | float | lidar, |
| float | angle ) |
Definition at line 222 of file rover_obstacles.c.
References compute_cell_bayes(), max_distance, N_COL_GRID, N_ROW_GRID, obstacle_grid, obtain_cell_xy(), FloatEulers::psi, rover_pos, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), and update_line_bayes().
Definition at line 109 of file rover_obstacles.c.
References DefaultPeriodic, L0, N_COL_GRID, N_ROW_GRID, obstacle_grid, register_periodic_telemetry(), send_grid_init(), WaypointX, and WaypointY.
Definition at line 136 of file rover_obstacles.c.
References DECAY, DefaultChannel, DefaultDevice, DefaultPeriodic, lidar_cb(), lidar_ev, N_COL_GRID, N_ROW_GRID, obstacle_grid, OBSTACLES_RECEIVE_ID, P_FREE, P_OCC, P_T, register_periodic_telemetry(), send_grid_init(), WaypointX, and WaypointY.
| void ins_update_lidar | ( | float | distance, |
| float | angle ) |
Definition at line 370 of file rover_obstacles.c.
References convert_walls_to_ltp(), fill_bayesian_cell(), FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), stateIsLocalCoordinateValid(), wall_system, EnuCoor_f::x, FloatVect2::x, EnuCoor_f::y, and FloatVect2::y.
Referenced by lidar_cb().
Definition at line 407 of file rover_obstacles.c.
References ins_update_lidar().
Referenced by init_grid_4().
| void obtain_cell_xy | ( | float | px, |
| float | py, | ||
| int * | cell_x, | ||
| int * | cell_y ) |
Definition at line 182 of file rover_obstacles.c.
References obstacle_grid.
Referenced by fill_bayesian_cell(), fill_cell(), and fill_free_cells().
|
static |
Definition at line 95 of file rover_obstacles.c.
References dev, and obstacle_grid.
Referenced by init_grid(), and init_grid_4().
|
static |
Definition at line 80 of file rover_obstacles.c.
References dev, N_COL_GRID, N_ROW_GRID, and obstacle_grid.
| void update_cell | ( | int | x, |
| int | y, | ||
| int | new_value ) |
Definition at line 306 of file rover_obstacles.c.
References DefaultChannel, DefaultDevice, LT, obstacle_grid, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by compute_cell_bayes(), and decay_map().
| void update_line_bayes | ( | int | x0, |
| int | y0, | ||
| int | x1, | ||
| int | y1 ) |
Definition at line 289 of file rover_obstacles.c.
References compute_cell_bayes().
Referenced by fill_bayesian_cell(), and fill_free_cells().
| uint8_t grid_block_size = GRID_BLOCK_SIZE |
Definition at line 76 of file rover_obstacles.c.
|
static |
Definition at line 59 of file rover_obstacles.c.
Referenced by decay_map().
|
static |
Definition at line 73 of file rover_obstacles.c.
Referenced by check_probs(), and compute_cell_bayes().
|
static |
Definition at line 63 of file rover_obstacles.c.
Referenced by init_grid_4().
|
static |
Definition at line 73 of file rover_obstacles.c.
Referenced by check_probs(), and compute_cell_bayes().
|
static |
Definition at line 73 of file rover_obstacles.c.
Referenced by check_probs(), and update_cell().
| float max_distance = 5 |
Definition at line 65 of file rover_obstacles.c.
Referenced by fill_free_cells().
| world_grid obstacle_grid |
Definition at line 75 of file rover_obstacles.c.
Referenced by check_probs(), compute_cell_bayes(), decay_map(), fill_bayesian_cell(), fill_cell(), fill_free_cells(), init_grid(), init_grid_4(), obtain_cell_xy(), send_grid_init(), send_obstacle_grid(), and update_cell().
|
static |
Definition at line 70 of file rover_obstacles.c.
Referenced by check_probs().
|
static |
Definition at line 69 of file rover_obstacles.c.
Referenced by check_probs().
|
static |
Definition at line 71 of file rover_obstacles.c.
Referenced by check_probs().