52#ifndef OBSTACLES_RECEIVE_ID
53#define OBSTACLES_RECEIVE_ID ABI_BROADCAST
57#define DECAY_INTERVAL 5000
83 pprz_msg_send_OBSTACLE_GRID(trans,
dev, AC_ID,
97 pprz_msg_send_GRID_INIT(trans,
dev, AC_ID,
127#if PERIODIC_TELEMETRY
161#if PERIODIC_TELEMETRY
168 DOWNLINK_SEND_GRID_INIT(
190 *cell_x = (*cell_x >= 0) ? *cell_x : 0;
191 *cell_y = (*cell_y >= 0) ? *cell_y : 0;
240 float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
267 }
else if (*cell > 0) {
270 }
else if (*cell < 0) {
291 int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
292 int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
296 if (x0 == x1 && y0 == y1) {
break; }
299 if (e2 >= dy) { err += dy; x0 += sx; }
300 if (e2 <= dx) { err += dx; y0 += sy; }
312 uint8_t old_state = (old_value >
LT) ? 1 : (old_value < -
LT) ? 2 : 0;
313 uint8_t new_state = (new_value >
LT) ? 1 : (new_value < -
LT) ? 2 : 0;
316 *cell = (
int8_t)new_value;
318 if (old_state != new_state) {
335 int updated = *cell + delta;
382 if (distance == 0.0) {
392 float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
395 obstacle.
x = x_rover + (distance * cosf(corrected_angle));
396 obstacle.
y = y_rover + (distance * sinf(corrected_angle));
408 uint32_t stamp __attribute__((unused)),
409 float distance,
float angle)
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
static struct uart_periph * dev
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
void convert_walls_to_ltp(void)
struct WallSystem wall_system
static struct NedCoor_d rover_pos
Physical model structures.
Paparazzi floating point math for geodetic calculations.
vector in East North Up coordinates Units: meters
Rover navigation functions.
static void send_grid_init(struct transport_tx *trans, struct link_device *dev)
static void lidar_cb(uint8_t sender_id, uint32_t stamp, float distance, float angle)
void obtain_cell_xy(float px, float py, int *cell_x, int *cell_y)
static void send_obstacle_grid(struct transport_tx *trans, struct link_device *dev)
void fill_bayesian_cell(float px, float py)
void init_grid_4(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
void ins_update_lidar(float distance, float angle)
#define OBSTACLES_RECEIVE_ID
void fill_free_cells(float lidar, float angle)
void update_cell(int x, int y, int new_value)
void compute_cell_bayes(int x, int y, bool is_occupied)
static abi_event lidar_ev
void init_grid(uint8_t pa, uint8_t pb)
void fill_cell(float px, float py)
void update_line_bayes(int x0, int y0, int x1, int y1)
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.