38#error "OBSTACLE_WALLS is not defined. Please define it in your configuration."
52 float denom = (
B->x -
A->x) * sinf(theta) - (
B->y -
A->y) * cosf(theta);
55 if (fabsf(denom) < 1e-6f) {
60 float t = ((
A->y -
B->y) * (
A->x -
P->x) - (
A->x -
B->x) * (
A->y -
P->y)) / denom;
61 float s = (cosf(theta) * (
A->y -
P->y) - sinf(theta) * (
A->x -
P->x)) / denom;
65 if (t > 0.0f &&
s >= 0.0f &&
s <= 1.0f) {
82 float length_sq = vecAB.
x * vecAB.
x + vecAB.
y * vecAB.
y;
83 float dot_product = vecAP.
x * vecAB.
x + vecAP.
y * vecAB.
y;
84 float t = (length_sq != 0) ? dot_product / length_sq : 0.0f;
86 t = (t < 0.0f) ? 0.0f : ((t > 1.0f) ? 1.0f : t);
88 C->
x =
A->x + t * vecAB.
x;
89 C->
y =
A->y + t * vecAB.
y;
91 float dx =
P->x - C->
x;
92 float dy =
P->y - C->
y;
93 return sqrtf(dx * dx + dy * dy);
104 float min_distance = FLT_MAX;
121 if (distance < min_distance) {
122 psi = atan2f(-(p2.
y - p1.
y), p2.
x - p1.
x);
123 min_distance = distance;
124 *nearest_point = aux_point;
150 for (
int w = 0; w < wall_count; w++) {
155 for (
int p = 0;
p < wall->
count;
p++) {
174 struct NedCoor_f ned = {0.0f, 0.0f, 0.0f};
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
INS for rotorcrafts combining vertical and horizontal filters.
const struct WallConfig obstacle_walls[]
static float distance_to_segment(const struct FloatVect2 *P, const struct FloatVect2 *A, const struct FloatVect2 *B, struct FloatVect2 *C)
float find_nearest_wall(const struct FloatVect2 *obstacle_pos, struct FloatVect2 *nearest_point)
float distance_to_wall(float theta, const struct FloatVect2 *P, const struct FloatVect2 *A, const struct FloatVect2 *B)
void convert_walls_to_ltp(void)
struct WallSystem wall_system
struct FloatVect2 points_ltp[MAX_POINTS]
struct LlaCoor_f points_wgs84[MAX_POINTS]
struct ObstaclePoint points[MAX_POINTS]
Paparazzi generic algebra macros.
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
driver for the TFMini lidar
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.