diff --git a/conf/abi.xml b/conf/abi.xml
index e567827741..834808e8fe 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -265,6 +265,7 @@
+
diff --git a/conf/airframes/UCM/conf.xml b/conf/airframes/UCM/conf.xml
new file mode 100644
index 0000000000..e85a35c750
--- /dev/null
+++ b/conf/airframes/UCM/conf.xml
@@ -0,0 +1,13 @@
+
+
+
diff --git a/conf/airframes/UCM/rover_lidar.xml b/conf/airframes/UCM/rover_lidar.xml
new file mode 100644
index 0000000000..2de5dddd18
--- /dev/null
+++ b/conf/airframes/UCM/rover_lidar.xml
@@ -0,0 +1,231 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/UCM/mission_lidar.xml b/conf/flight_plans/UCM/mission_lidar.xml
new file mode 100644
index 0000000000..d5a9a9ddf6
--- /dev/null
+++ b/conf/flight_plans/UCM/mission_lidar.xml
@@ -0,0 +1,92 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/lidar_correction.xml b/conf/modules/lidar_correction.xml
new file mode 100644
index 0000000000..68fbb094ec
--- /dev/null
+++ b/conf/modules/lidar_correction.xml
@@ -0,0 +1,26 @@
+
+
+
+
+
+ LIDAR correction for SLAM applications
+ Includes functions used to simulate the LIDAR data
+ (Optional module for advanced LIDAR processing)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/modules/lidar_tfmini.xml b/conf/modules/lidar_tfmini.xml
index c041f45bfa..5e9f15d8e6 100644
--- a/conf/modules/lidar_tfmini.xml
+++ b/conf/modules/lidar_tfmini.xml
@@ -3,19 +3,26 @@
- TFMini Lidar using a single UART for communication
+ TFMini Lidar driver with separate implementations for:
+ - Real hardware using a single UART for communication (ap target)
+ - NPS simulator (nps target)
-
-
+
+
+
+
+
+
+
-
-
+
+
@@ -29,7 +36,9 @@
-
+
+
+
@@ -39,14 +48,36 @@
-
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/obstacle_rover.xml b/conf/modules/obstacle_rover.xml
new file mode 100644
index 0000000000..831eca8285
--- /dev/null
+++ b/conf/modules/obstacle_rover.xml
@@ -0,0 +1,36 @@
+
+
+
+
+
+ Obstacle rover grid
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/servo_lidar.xml b/conf/modules/servo_lidar.xml
new file mode 100644
index 0000000000..2bad5a1089
--- /dev/null
+++ b/conf/modules/servo_lidar.xml
@@ -0,0 +1,33 @@
+
+
+
+
+
+ Servo-mounted LIDAR scanning system
+ (Uses tfmini driver and provides scanning functionality)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/telemetry/default_lidar_telemetry.xml b/conf/telemetry/default_lidar_telemetry.xml
new file mode 100644
index 0000000000..307110b55b
--- /dev/null
+++ b/conf/telemetry/default_lidar_telemetry.xml
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rover/obstacles/rover_obstacles.c b/sw/airborne/firmwares/rover/obstacles/rover_obstacles.c
new file mode 100644
index 0000000000..5cb34a86ea
--- /dev/null
+++ b/sw/airborne/firmwares/rover/obstacles/rover_obstacles.c
@@ -0,0 +1,412 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file rover_obstacles.c
+ * @brief functions to create a grid map
+ *
+ */
+
+
+// Depends of tfmini lidar(could be replaced by other lidar)
+
+#include "rover_obstacles.h"
+#include "math/pprz_geodetic_float.h"
+
+#include "modules/datalink/telemetry.h"
+#include "state.h"
+
+#include "firmwares/rover/navigation.h"
+
+
+// Probability Map
+#define P_FREE 0.4 // cell observed as free (negative log-odds)
+#define P_OCC 0.7 // cell observed as occupied (positive log-odds)
+#define L_MIN -127 // minimum saturation
+#define L_MAX 127 // maximum saturation
+#define L0 0 // initial value (unknown)
+#define P_T 0.95 // Threshold to consider a cell occupied/free
+
+#define SCALE 20.0f // scaling of log-odds float to int8_t --> With 20, max prob 0.995
+
+
+// Abi call
+#ifndef OBSTACLES_RECEIVE_ID
+#define OBSTACLES_RECEIVE_ID ABI_BROADCAST
+#endif
+
+
+#define DECAY_INTERVAL 5000 // ms for obstacle probability to decay
+#define DECAY 5 // Decay per second
+static uint32_t last_s = 0;
+
+
+#include "modules/core/abi.h"
+static abi_event lidar_ev;
+static void lidar_cb(uint8_t sender_id, uint32_t stamp, float distance, float angle);
+float max_distance = 5;
+
+
+// Global variables (to avoid recalculating log all the time)
+static float POCC = 0;
+static float PFREE = 0;
+static float PT = 0;
+
+static int8_t LT, LOCC, LFREE;
+
+world_grid obstacle_grid;
+uint8_t grid_block_size = GRID_BLOCK_SIZE; // This is only used in CBF
+
+
+#if PERIODIC_TELEMETRY
+static void send_obstacle_grid(struct transport_tx *trans, struct link_device *dev)
+{
+ // Send all cols from obstacle_grid.now_row in a cyclic pattern
+ pprz_msg_send_OBSTACLE_GRID(trans, dev, AC_ID,
+ &obstacle_grid.dx,
+ &obstacle_grid.dy,
+ &obstacle_grid.xmin,
+ &obstacle_grid.xmax,
+ &obstacle_grid.ymin,
+ &obstacle_grid.ymax,
+ &obstacle_grid.now_row,
+ N_COL_GRID, obstacle_grid.world[obstacle_grid.now_row]);
+
+ obstacle_grid.now_row = (obstacle_grid.now_row + 1) % N_ROW_GRID;
+}
+static void send_grid_init(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_GRID_INIT(trans, dev, AC_ID,
+ &obstacle_grid.dx,
+ &obstacle_grid.dy,
+ &obstacle_grid.xmin,
+ &obstacle_grid.xmax,
+ &obstacle_grid.ymin,
+ &obstacle_grid.ymax,
+ &obstacle_grid.map.LT
+ );
+}
+#endif
+
+void init_grid(uint8_t pa, uint8_t pb)
+{
+
+ int i, j;
+ for (i = 0; i < N_ROW_GRID; i++) {
+ for (j = 0; j < N_COL_GRID; j++) {
+ obstacle_grid.world[i][j] = L0;
+ }
+ }
+ // Rows in X, cols in Y
+ obstacle_grid.xmin = WaypointX(pa);
+ obstacle_grid.xmax = WaypointX(pb);
+ obstacle_grid.ymin = WaypointY(pa);
+ obstacle_grid.ymax = WaypointY(pb);
+ obstacle_grid.dx = (obstacle_grid.xmax - obstacle_grid.xmin) / ((float)N_COL_GRID);
+ obstacle_grid.dy = (obstacle_grid.ymax - obstacle_grid.ymin) / ((float)N_ROW_GRID);
+ obstacle_grid.now_row = 0;
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GRID_INIT, send_grid_init);
+#endif
+ obstacle_grid.is_ready = 1;
+
+}
+
+// Same as init_grid but with 4 waypoints (you can give the 4 wp in any order,
+// but you need to have them in the correct order in the flightplan for painting the border in the GCS).
+void init_grid_4(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
+{
+ float xmin = fminf(fminf(WaypointX(wp1), WaypointX(wp2)), fminf(WaypointX(wp3), WaypointX(wp4)));
+ float xmax = fmaxf(fmaxf(WaypointX(wp1), WaypointX(wp2)), fmaxf(WaypointX(wp3), WaypointX(wp4)));
+ float ymin = fminf(fminf(WaypointY(wp1), WaypointY(wp2)), fminf(WaypointY(wp3), WaypointY(wp4)));
+ float ymax = fmaxf(fmaxf(WaypointY(wp1), WaypointY(wp2)), fmaxf(WaypointY(wp3), WaypointY(wp4)));
+
+ obstacle_grid.xmin = xmin;
+ obstacle_grid.xmax = xmax;
+ obstacle_grid.ymin = ymin;
+ obstacle_grid.ymax = ymax;
+
+ obstacle_grid.dx = (xmax - xmin) / ((float)N_COL_GRID);
+ obstacle_grid.dy = (ymax - ymin) / ((float)N_ROW_GRID);
+
+ obstacle_grid.now_row = 0;
+ obstacle_grid.is_ready = 1;
+
+ obstacle_grid.map.threshold = (float) P_T;
+ obstacle_grid.map.occ = (float) P_OCC;
+ obstacle_grid.map.free = (float) P_FREE;
+ obstacle_grid.map.decay = (uint8_t) DECAY;
+
+ memset(obstacle_grid.world, 0, sizeof(obstacle_grid.world));
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GRID_INIT, send_grid_init);
+#endif
+
+ AbiBindMsgOBSTACLE_DETECTION(OBSTACLES_RECEIVE_ID, &lidar_ev, lidar_cb);
+
+ // Send the message to the GCS
+ DOWNLINK_SEND_GRID_INIT(
+ DefaultChannel,
+ DefaultDevice,
+ &obstacle_grid.dx,
+ &obstacle_grid.dy,
+ &obstacle_grid.xmin,
+ &obstacle_grid.xmax,
+ &obstacle_grid.ymin,
+ &obstacle_grid.ymax,
+ &obstacle_grid.map.LT
+ );
+}
+
+
+void obtain_cell_xy(float px, float py, int *cell_x, int *cell_y)
+{
+
+ // Must be >= 0, xmin <= px <= xmax
+ // ymin <= py <= ymax
+ *cell_x = (int)((px - obstacle_grid.xmin) / obstacle_grid.dx); // Like floor
+ *cell_y = (int)((py - obstacle_grid.ymin) / obstacle_grid.dy);
+
+ *cell_x = (*cell_x >= 0) ? *cell_x : 0;
+ *cell_y = (*cell_y >= 0) ? *cell_y : 0;
+}
+
+void fill_cell(float px, float py)
+{
+
+ int cx, cy;
+ obtain_cell_xy(px, py, &cx, &cy);
+ obstacle_grid.world[cy][cx] = 1; // Row, col
+
+}
+
+void fill_bayesian_cell(float px, float py)
+{
+
+ if (!obstacle_grid.is_ready) { return; }
+
+ int cx, cy;
+ obtain_cell_xy(px, py, &cx, &cy);
+
+ // Gets the cell where the rover is
+ int rx, ry;
+ struct EnuCoor_f rover_pos;
+ rover_pos = *stateGetPositionEnu_f();
+ obtain_cell_xy(rover_pos.x, rover_pos.y, &rx, &ry);
+
+ update_line_bayes(rx, ry, cx, cy); // Free between rover and obstacle
+ compute_cell_bayes(cx, cy, true); // Ocupied at the last point
+}
+
+// Fills the free cells when there is no lidar measurement
+void fill_free_cells(float lidar, float angle)
+{
+ if (!obstacle_grid.is_ready) { return; }
+
+ // Gets the cell where the rover is
+ int rx, ry;
+ struct EnuCoor_f rover_pos;
+ rover_pos = *stateGetPositionEnu_f();
+ obtain_cell_xy(rover_pos.x, rover_pos.y, &rx, &ry);
+ if (rx < 0 || rx >= N_COL_GRID || ry < 0 || ry >= N_ROW_GRID) {
+ return;
+ }
+
+ // If there is no lidar measurement, mark cells as free
+ if (lidar == 0.0f) {
+ // Calculates the fictitious obstacle
+ int cx, cy;
+ float theta = stateGetNedToBodyEulers_f()->psi;
+ float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
+
+ float px = rover_pos.x + (max_distance * cosf(corrected_angle));
+ float py = rover_pos.y + (max_distance * sinf(corrected_angle));
+
+ obtain_cell_xy(px, py, &cx, &cy);
+
+ update_line_bayes(rx, ry, cx, cy); // Free between rover and obstacle
+ return;
+ } else {
+ compute_cell_bayes(rx, ry, false); // Free at the end point
+ }
+}
+
+
+// Function for the map to forget the obstacles
+void decay_map()
+{
+ uint32_t now_s = get_sys_time_msec();
+
+ if (now_s > (last_s + DECAY_INTERVAL)) {
+ last_s = now_s;
+ for (int y = 0; y < N_ROW_GRID; y++) {
+ for (int x = 0; x < N_COL_GRID; x++) {
+ int8_t *cell = &obstacle_grid.world[y][x];
+ if (*cell == 0) {
+ continue;
+ } else if (*cell > 0) {
+ int updated = (*cell > obstacle_grid.map.decay) ? *cell - obstacle_grid.map.decay : 0;
+ update_cell(x, y, updated);
+ } else if (*cell < 0) {
+ int updated = (*cell < - obstacle_grid.map.decay) ? *cell + obstacle_grid.map.decay : 0;
+ update_cell(x, y, updated);
+ }
+ }
+ }
+ }
+}
+
+
+
+/*******************************************************************************
+ * *
+ * Aux functions *
+ * *
+ ******************************************************************************/
+
+
+// Bresenham algorithm
+void update_line_bayes(int x0, int y0, int x1, int y1)
+{
+ int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
+ int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
+ int err = dx + dy;
+
+ while (1) {
+ if (x0 == x1 && y0 == y1) { break; }
+ compute_cell_bayes(x0, y0, false); // Libre
+ int e2 = 2 * err;
+ if (e2 >= dy) { err += dy; x0 += sx; }
+ if (e2 <= dx) { err += dx; y0 += sy; }
+ }
+}
+
+
+// Decide to send the cell (0 unknown, 1 occupied, 2 free)
+void update_cell(int x, int y, int new_value)
+{
+
+ int8_t *cell = &obstacle_grid.world[y][x];
+ int8_t old_value = *cell;
+
+ uint8_t old_state = (old_value > LT) ? 1 : (old_value < -LT) ? 2 : 0;
+ uint8_t new_state = (new_value > LT) ? 1 : (new_value < -LT) ? 2 : 0;
+ obstacle_grid.map.LT = LT; // For the GCS
+
+ *cell = (int8_t)new_value;
+
+ if (old_state != new_state) {
+ DOWNLINK_SEND_GRID_CHANGES(DefaultChannel, DefaultDevice, &y, &x, &new_value);
+ // printf("Cell (%d, %d) updated from %d to %d (delta: %d)\n", x, y, old_value, *cell, delta);
+ }
+}
+
+
+void compute_cell_bayes(int x, int y, bool is_occupied)
+{
+ if (x < 0 || x >= N_COL_GRID || y < 0 || y >= N_ROW_GRID) {
+ return;
+ }
+ int8_t *cell = &obstacle_grid.world[y][x];
+
+ check_probs();
+
+ int delta = is_occupied ? LOCC : LFREE;
+ int updated = *cell + delta;
+ if (updated > L_MAX) { updated = L_MAX; }
+ if (updated < L_MIN) { updated = L_MIN; }
+
+ update_cell(x, y, updated);
+}
+
+
+//int8_t *LOCC, int8_t *LFREE, int8_t *LT
+void check_probs(void)
+{
+
+ if ((obstacle_grid.map.occ != POCC) || (obstacle_grid.map.free != PFREE)) {
+ LOCC = (int8_t)(SCALE * logf(obstacle_grid.map.occ / (1.0f - obstacle_grid.map.occ)));
+ LFREE = (int8_t)(SCALE * logf(obstacle_grid.map.free / (1.0f - obstacle_grid.map.free)));
+ POCC = obstacle_grid.map.occ;
+ PFREE = obstacle_grid.map.free;
+ // printf("LOCC: %d, LFREE: %d\n", *LOCC, *LFREE);
+ }
+
+ if (obstacle_grid.map.threshold != PT) {
+ LT = (int8_t)(SCALE * logf(obstacle_grid.map.threshold / (1.0f - obstacle_grid.map.threshold)));
+ PT = obstacle_grid.map.threshold;
+ }
+}
+
+
+
+/*******************************************************************************
+ * *
+ * Testing functions *
+ * *
+ ******************************************************************************/
+
+
+void ins_update_lidar(float distance, float angle)
+{
+
+ if (!stateIsLocalCoordinateValid()) {
+ return;
+ }
+
+ if (!wall_system.converted_to_ltp) {
+ convert_walls_to_ltp();
+ }
+
+ // In case there is real measured for the Lidar
+ if (distance == 0.0) {
+ return;
+ }
+
+ // Everything is in ENU
+ float x_rover = stateGetPositionEnu_f()->x;
+ float y_rover = stateGetPositionEnu_f()->y;
+
+
+ float theta = stateGetNedToBodyEulers_f()->psi;
+ float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
+
+ struct FloatVect2 obstacle = {0.0, 0.0};
+ obstacle.x = x_rover + (distance * cosf(corrected_angle));
+ obstacle.y = y_rover + (distance * sinf(corrected_angle));
+
+ // Fill the grid
+#ifdef USE_GRID
+ fill_bayesian_cell(obstacle.x, obstacle.y);
+#endif
+
+}
+
+
+
+static void lidar_cb(uint8_t __attribute__((unused)) sender_id,
+ uint32_t stamp __attribute__((unused)),
+ float distance, float angle)
+{
+ ins_update_lidar(distance, angle);
+}
\ No newline at end of file
diff --git a/sw/airborne/firmwares/rover/obstacles/rover_obstacles.h b/sw/airborne/firmwares/rover/obstacles/rover_obstacles.h
new file mode 100644
index 0000000000..5a7e4ce60e
--- /dev/null
+++ b/sw/airborne/firmwares/rover/obstacles/rover_obstacles.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file rover_obstacles.hs
+ * @brief functions to create a grid map
+ *
+ */
+
+
+#ifndef ROVER_OBSTACLES_H
+#define ROVER_OBSTACLES_H
+
+
+#ifndef N_COL_GRID
+#define N_COL_GRID 100
+#endif
+
+#ifndef N_ROW_GRID
+#define N_ROW_GRID 100
+#endif
+
+#include "std.h"
+
+
+typedef struct{
+ int8_t LT; // Threshold for occupied/cells (log-odds)
+ float threshold; // Threshold for occupied/free cells
+ float occ; // Occupied cells probability
+ float free; // Free cells probability
+ uint8_t decay; // Decay Probability (log-odds)
+} bayesian_map;
+
+
+typedef struct{
+ int8_t world[N_ROW_GRID][N_COL_GRID];
+ float xmin;
+ float xmax;
+ float ymin;
+ float ymax;
+ float home[2];
+ float dx;
+ float dy;
+ uint16_t now_row;
+ int is_ready;
+ bayesian_map map;
+} world_grid;
+
+extern world_grid obstacle_grid;
+
+#define GRID_BLOCK_SIZE 4
+extern uint8_t grid_block_size; // This is only used in CBF
+
+
+extern void init_grid(uint8_t pa, uint8_t pb);
+extern void init_grid_4(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4);
+extern void obtain_cell_xy(float px, float py, int *cell_x, int *cell_y);
+
+extern void fill_cell(float px, float py);
+extern void fill_bayesian_cell(float px, float py);
+extern void fill_free_cells(float lidar, float angle);
+
+extern void decay_map(void);
+extern void update_line_bayes(int x0, int y0, int x1, int y1);
+extern void update_cell(int x, int y, int new_value);
+extern void compute_cell_bayes(int x, int y, bool is_occupied);
+extern void check_probs(void);
+
+
+#endif // ROVER_OBSTACLES_H
diff --git a/sw/airborne/modules/lidar/slam/lidar_correction.c b/sw/airborne/modules/lidar/slam/lidar_correction.c
new file mode 100644
index 0000000000..0f8f5487ad
--- /dev/null
+++ b/sw/airborne/modules/lidar/slam/lidar_correction.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas Fernández
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+// ------------------------------------
+
+
+#include "lidar_correction.h"
+#include "modules/lidar/tfmini.h"
+#include "modules/ins/ins_int.h"
+
+#include "math/pprz_algebra.h"
+#include
+#include
+#include
+
+
+struct WallSystem wall_system; // Global wall system
+
+#ifndef OBSTACLE_WALLS
+#error "OBSTACLE_WALLS is not defined. Please define it in your configuration."
+#endif
+
+#ifdef USE_GRID
+#include "firmwares/rover/obstacles/rover_obstacles.h"
+#endif
+
+
+// Calculates the distance from a point P to a wall defined by points A and B
+// taking into account the orientation theta.
+float distance_to_wall(float theta, const struct FloatVect2 *P, const struct FloatVect2 *A,
+ const struct FloatVect2 *B)
+{
+
+ float denom = (B->x - A->x) * sinf(theta) - (B->y - A->y) * cosf(theta);
+
+ // If the denominator is 0, the lines are parallel
+ if (fabsf(denom) < 1e-6f) {
+ return 0;
+ }
+
+ // Calculate t (distance) and s (wall parameter)
+ float t = ((A->y - B->y) * (A->x - P->x) - (A->x - B->x) * (A->y - P->y)) / denom;
+ float s = (cosf(theta) * (A->y - P->y) - sinf(theta) * (A->x - P->x)) / denom;
+
+
+ // Check if the intersection is valid
+ if (t > 0.0f && s >= 0.0f && s <= 1.0f) {
+ return t;
+ } else {
+ return 0;
+ }
+}
+
+
+
+// Calculates the distance from a point P to a segment AB and returns the closest point C
+static float distance_to_segment(const struct FloatVect2 *P, const struct FloatVect2 *A,
+ const struct FloatVect2 *B, struct FloatVect2 *C)
+{
+
+ struct FloatVect2 vecAB = {B->x - A->x, B->y - A->y};
+ struct FloatVect2 vecAP = {P->x - A->x, P->y - A->y};
+
+ float length_sq = vecAB.x * vecAB.x + vecAB.y * vecAB.y;
+ float dot_product = vecAP.x * vecAB.x + vecAP.y * vecAB.y;
+ float t = (length_sq != 0) ? dot_product / length_sq : 0.0f;
+
+ t = (t < 0.0f) ? 0.0f : ((t > 1.0f) ? 1.0f : t);
+
+ C->x = A->x + t * vecAB.x;
+ C->y = A->y + t * vecAB.y;
+
+ float dx = P->x - C->x;
+ float dy = P->y - C->y;
+ return sqrtf(dx * dx + dy * dy);
+}
+
+
+float find_nearest_wall(const struct FloatVect2 *obstacle_pos, struct FloatVect2 *nearest_point)
+{
+
+ if (!wall_system.converted_to_ltp) {
+ return FLT_MAX;
+ }
+
+ float min_distance = FLT_MAX;
+ float psi = 10; // psi = [-pi, pi]
+
+ // Iterate over all walls
+ for (uint8_t w = 0; w < wall_system.wall_count; w++) {
+ struct Wall *wall = &wall_system.walls[w];
+
+ // Iterate over all segments of the wall
+ for (uint8_t p = 0; p < wall->count - 1; p++) {
+ struct FloatVect2 p1 = wall->points_ltp[p];
+ struct FloatVect2 p2 = wall->points_ltp[p + 1];
+
+ // Calculate distance to the line segment
+ // p1 and p2 are the ends of the wall. The result is stored in aux_point
+ struct FloatVect2 aux_point = {0.0f, 0.0f};
+ float distance = distance_to_segment(obstacle_pos, &p1, &p2, &aux_point);
+
+ if (distance < min_distance) {
+ psi = atan2f(-(p2.y - p1.y), p2.x - p1.x);
+ min_distance = distance;
+ *nearest_point = aux_point;
+ }
+ }
+ }
+
+ return min_distance;
+}
+
+
+
+/*******************************************************************************
+ * *
+ * Wall functions *
+ * *
+ ******************************************************************************/
+
+// Include the obstacles configuration file
+const struct WallConfig obstacle_walls[] = OBSTACLE_WALLS;
+
+// Parse of obstacles (defined in the map file)
+void init_walls(void)
+{
+ uint8_t wall_count = sizeof(obstacle_walls) / sizeof(obstacle_walls[0]);
+
+ wall_system.wall_count = wall_count;
+
+ for (int w = 0; w < wall_count; w++) {
+ const struct WallConfig *cfg = &obstacle_walls[w];
+ struct Wall *wall = &wall_system.walls[w];
+ wall->count = cfg->count;
+
+ for (int p = 0; p < wall->count; p++) {
+ wall->points_wgs84[p] = (struct LlaCoor_f){
+ .lat = RadOfDeg(cfg->points[p].lat_deg),
+ .lon = RadOfDeg(cfg->points[p].lon_deg),
+ .alt = cfg->points[p].alt
+ };
+ }
+ }
+
+ wall_system.converted_to_ltp = false;
+}
+
+
+void convert_walls_to_ltp(void)
+{
+ if (wall_system.converted_to_ltp || !stateIsLocalCoordinateValid()) { return; }
+
+ for (int w = 0; w < wall_system.wall_count; w++) {
+ for (int p = 0; p < wall_system.walls[w].count; p++) {
+ struct NedCoor_f ned = {0.0f, 0.0f, 0.0f};
+ ned_of_lla_point_f(&ned, stateGetNedOrigin_f(), &wall_system.walls[w].points_wgs84[p]);
+
+ wall_system.walls[w].points_ltp[p].x = ned.y;
+ wall_system.walls[w].points_ltp[p].y = ned.x;
+ }
+ wall_system.walls[w].converted = true;
+ }
+ wall_system.converted_to_ltp = true;
+}
+
diff --git a/sw/airborne/modules/lidar/slam/lidar_correction.h b/sw/airborne/modules/lidar/slam/lidar_correction.h
new file mode 100644
index 0000000000..076e5b65ad
--- /dev/null
+++ b/sw/airborne/modules/lidar/slam/lidar_correction.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas Fernández
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+// ------------------------------------
+
+
+
+#ifndef LIDAR_CORRECTION_H
+#define LIDAR_CORRECTION_H
+
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_geodetic_float.h"
+
+#define MAX_WALLS 10 // Máx number of walls
+#define MAX_POINTS 5 // Máx number of points in a wall
+
+
+// Structs to include the obstacles configuration file
+struct ObstaclePoint {
+ float lat_deg;
+ float lon_deg;
+ float alt;
+};
+
+struct WallConfig {
+ uint8_t count;
+ struct ObstaclePoint points[MAX_POINTS];
+};
+
+
+
+struct Wall {
+ struct LlaCoor_f points_wgs84[MAX_POINTS];
+ struct FloatVect2 points_ltp[MAX_POINTS];
+ uint8_t count;
+ bool converted;
+};
+
+struct WallSystem {
+ struct Wall walls[MAX_WALLS];
+ uint8_t wall_count;
+ bool converted_to_ltp;
+};
+
+
+extern struct WallSystem wall_system;
+extern void init_walls(void); // Init Obstacles
+extern void convert_walls_to_ltp(void);
+extern float find_nearest_wall(const struct FloatVect2 *obstacle_pos, struct FloatVect2 *nearest_point);
+extern float distance_to_wall(float theta, const struct FloatVect2 *P, const struct FloatVect2 *A,
+ const struct FloatVect2 *B);
+
+#endif // LIDAR_CORRECTION_H
diff --git a/sw/airborne/modules/lidar/slam/servo_lidar.c b/sw/airborne/modules/lidar/slam/servo_lidar.c
new file mode 100644
index 0000000000..917b1ac415
--- /dev/null
+++ b/sw/airborne/modules/lidar/slam/servo_lidar.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/lidar/slam/servo_lidar.c
+ * @brief driver for the servo to move the lidar
+ *
+ */
+
+ // Depends on the lidar (it doesnt make any sense without it)
+
+#include "mcu_periph/uart.h"
+#include "modules/core/abi.h"
+#include "modules/datalink/downlink.h"
+#include "servo_lidar.h"
+#include "modules/lidar/tfmini.h"
+
+
+
+#ifndef SERVO_LIDAR_SPEED
+#define SERVO_LIDAR_SPEED 5
+#endif
+
+bool enable_servo = false;
+
+struct ServoLidar servoLidar;
+
+void servoLidar_init(void) {
+ servoLidar.enabled = false;
+ servoLidar.speed = SERVO_LIDAR_SPEED; // default speed
+ servoLidar.position = 0;
+ servoLidar.angle = 0;
+ servoLidar.direction = 0;
+ servoLidar.last_update = 0;
+}
+
+
+#ifdef COMMAND_SERVO
+void servoLidar_periodic(void) {
+ if (get_sys_time_msec() > servoLidar.last_update + servoLidar.speed) {
+ servoLidar.last_update = get_sys_time_msec();
+
+ if(servoLidar.enabled) {
+ // Update servo position
+ servoLidar.position += (servoLidar.direction == 0) ? 100 : -100;
+ if (servoLidar.position >= MAX_PPRZ*0.8 || servoLidar.position <= -MAX_PPRZ*0.8) {
+ servoLidar.direction ^= 1;
+ }
+
+ // Set servo command
+ commands[COMMAND_SERVO] = servoLidar.position;
+ servoLidar.angle = PWM2ANGLE(servoLidar.position);
+
+ // Send ABI message
+ AbiSendMsgOBSTACLE_DETECTION(AGL_LIDAR_TFMINI_ID, tfmini.distance, servoLidar.angle, 0);
+ }
+ else {
+ commands[COMMAND_SERVO] = 0; // Center servo when disabled
+ }
+ }
+}
+#else
+void servoLidar_periodic(void) {
+ // No servo functionality; do nothing
+ #error "You need to define COMMAND_SERVO to use servoLidar_periodic()"
+}
+#endif
\ No newline at end of file
diff --git a/sw/airborne/modules/lidar/slam/servo_lidar.h b/sw/airborne/modules/lidar/slam/servo_lidar.h
new file mode 100644
index 0000000000..26e0eff227
--- /dev/null
+++ b/sw/airborne/modules/lidar/slam/servo_lidar.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/lidar/slam/servo_lidar.h
+ * @brief driver for the servo to move the lidar
+ *
+ */
+
+
+#ifndef SERVO_LIDAR_H
+#define SERVO_LIDAR_H
+
+#include "std.h"
+
+#define PWM2ANGLE(pwm) (((pwm) + MAX_PPRZ) * 90 / MAX_PPRZ) - 90
+
+struct ServoLidar {
+ bool enabled;
+ uint8_t speed;
+ int32_t position;
+ float angle;
+ uint8_t direction;
+ uint32_t last_update;
+};
+
+extern struct ServoLidar servoLidar;
+
+extern void servoLidar_init(void);
+extern void servoLidar_periodic(void);
+
+#endif
\ No newline at end of file
diff --git a/sw/airborne/modules/lidar/tfmini.c b/sw/airborne/modules/lidar/tfmini.c
index be8ced5cbf..213388c659 100644
--- a/sw/airborne/modules/lidar/tfmini.c
+++ b/sw/airborne/modules/lidar/tfmini.c
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2019 Freek van Tienen
+ * Copyright (C) 2025 Alejandro Rochas
*
* This file is part of paparazzi.
*
@@ -35,12 +36,24 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
+
+// Horizontal distance from the IMU to the LiDAR (in meters)
+#ifndef LIDAR_OFFSET
+#define LIDAR_OFFSET 0.0f
+#endif
+
+// Height of the LiDAR above the ground (in meters)
+#ifndef LIDAR_HEIGHT
+#define LIDAR_HEIGHT 0.0f
+#endif
+
+static float lidar_offset;
+static float lidar_height;
+
struct TFMini tfmini = {
.parse_status = TFMINI_INITIALIZE
};
-static void tfmini_parse(uint8_t byte);
-
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -67,16 +80,21 @@ void tfmini_init(void)
tfmini.update_agl = USE_TFMINI_AGL;
tfmini.compensate_rotation = TFMINI_COMPENSATE_ROTATION;
+ tfmini.is_rover = TFMINI_ROVER;
tfmini.strength = 0;
tfmini.distance = 0;
tfmini.parse_status = TFMINI_PARSE_HEAD;
+ lidar_offset = LIDAR_OFFSET;
+ lidar_height = LIDAR_HEIGHT;
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, tfmini_send_lidar);
#endif
}
+
/**
* Lidar event function
* Receive bytes from the UART port and parse them
@@ -88,10 +106,11 @@ void tfmini_event(void)
}
}
+
/**
* Parse the lidar bytes 1 by 1
*/
-static void tfmini_parse(uint8_t byte)
+void tfmini_parse(uint8_t byte)
{
switch (tfmini.parse_status) {
case TFMINI_INITIALIZE:
@@ -146,7 +165,6 @@ static void tfmini_parse(uint8_t byte)
case TFMINI_PARSE_CHECKSUM:
// When the CRC matches
if (tfmini.parse_crc == byte) {
- uint32_t now_ts = get_sys_time_usec();
tfmini.distance = tfmini.raw_dist / 100.f;
tfmini.strength = tfmini.raw_strength;
tfmini.mode = tfmini.raw_mode;
@@ -155,16 +173,31 @@ static void tfmini_parse(uint8_t byte)
if (tfmini.distance != 0xFFFF) {
// compensate AGL measurement for body rotation
if (tfmini.compensate_rotation) {
- float phi = stateGetNedToBodyEulers_f()->phi;
- float theta = stateGetNedToBodyEulers_f()->theta;
- float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
- tfmini.distance = tfmini.distance * gain;
+ // If it is a rover, we need to compensate the distance
+ if (tfmini.is_rover) {
+ float theta = stateGetNedToBodyEulers_f()->theta;
+ float ground_distance;
+ if (fabs(theta) < 0.01) {
+ ground_distance = 100; // If it is 0 it is straight
+ } else {
+ ground_distance = lidar_height / sinf(-theta) - lidar_offset;
+ }
+
+ if ((tfmini.distance >= ground_distance) && (ground_distance > 0)) {
+ tfmini.distance = 0;
+ }
+ }
+ // If it is not a rover (like a drone), we need to compensate the distance differently
+ else {
+ float phi = stateGetNedToBodyEulers_f()->phi;
+ float theta = stateGetNedToBodyEulers_f()->theta;
+ float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
+ tfmini.distance = tfmini.distance * gain;
+ }
}
- // send message (if requested)
- if (tfmini.update_agl) {
- AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, now_ts, tfmini.distance);
- }
+ // Send the AGL message
+ tfmini_send_abi();
}
}
@@ -178,3 +211,19 @@ static void tfmini_parse(uint8_t byte)
break;
}
}
+
+
+// Send the lidar message (AGL, and, if requested, OBSTACLE_DETECTION)
+void tfmini_send_abi(void)
+{
+ uint32_t now_ts = get_sys_time_usec();
+ if (tfmini.update_agl) {
+ AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, now_ts, tfmini.distance);
+ }
+#ifndef USE_SERVO_LIDAR
+ //send message (if there is not servo module)
+ AbiSendMsgOBSTACLE_DETECTION(AGL_LIDAR_TFMINI_ID, tfmini.distance, 0, 0);
+#endif
+}
+
+
diff --git a/sw/airborne/modules/lidar/tfmini.h b/sw/airborne/modules/lidar/tfmini.h
index ddc08461d8..0d509956d4 100644
--- a/sw/airborne/modules/lidar/tfmini.h
+++ b/sw/airborne/modules/lidar/tfmini.h
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2019 Freek van Tienen
+ * Copyright (C) 2025 Alejandro Rochas
*
* This file is part of paparazzi.
*
@@ -29,6 +30,7 @@
#include "std.h"
#include "mcu_periph/i2c.h"
+#include "math/pprz_algebra_float.h"
enum TFMiniParseStatus {
TFMINI_INITIALIZE,
@@ -56,13 +58,21 @@ struct TFMini {
uint8_t mode;
bool update_agl;
bool compensate_rotation;
+ bool is_rover;
};
extern struct TFMini tfmini;
extern void tfmini_init(void);
extern void tfmini_event(void);
-extern void tfmini_downlink(void);
+extern void tfmini_send_abi(void);
+
+#ifndef USE_NPS
+extern void tfmini_parse(uint8_t byte);
+#else
+extern void sim_overwrite_lidar(void);
+extern void setLidarDistance_f(float distance);
+#endif // USE_NPS
#endif /* LIDAR_TFMINI_H */
diff --git a/sw/airborne/modules/lidar/tfmini_nps.c b/sw/airborne/modules/lidar/tfmini_nps.c
new file mode 100644
index 0000000000..8967c7b9d7
--- /dev/null
+++ b/sw/airborne/modules/lidar/tfmini_nps.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/lidar/tfmini_nps.c
+ * @brief simulation of a lidar
+ *
+ */
+
+#include "tfmini.h"
+#include "state.h"
+
+// Messages
+#include "pprzlink/messages.h"
+#include "modules/datalink/downlink.h"
+#include "modules/core/abi.h"
+
+#define LIDAR_MIN_RANGE 0.1
+#define LIDAR_MAX_RANGE 12.0
+
+struct TFMini tfmini;
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+/**
+ * Downlink message lidar
+ */
+static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev)
+{
+ uint8_t status = (uint8_t) tfmini.parse_status;
+ pprz_msg_send_LIDAR(trans, dev, AC_ID,
+ &tfmini.distance,
+ &tfmini.mode,
+ &status);
+}
+
+#endif
+
+
+
+/**
+ * Initialization function
+ */
+void tfmini_init(void)
+{
+ tfmini.distance = 0;
+ tfmini.device = &((TFMINI_PORT).device);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, tfmini_send_lidar);
+#endif
+}
+
+
+void tfmini_event(void)
+{
+ sim_overwrite_lidar();
+}
+
+
+
+// If you want to use the lidar simulation, you need to use the lidar correction module
+void sim_overwrite_lidar(void)
+{
+ if (!stateIsLocalCoordinateValid()) { return; }
+
+ // Convert GPS position to NED coordinates
+ struct FloatVect2 pos = {0.0f, 0.0f};
+ struct NedCoor_i ned = {0.0f, 0.0f, 0.0f};
+ ned_of_lla_point_i(&ned, stateGetNedOrigin_i(), &gps.lla_pos);
+ pos.x = (float)(ned.y / 100.0f);
+ pos.y = (float)(ned.x / 100.0f);
+
+ // Calculate the angle of the rover (and servo if used)
+ float theta = M_PI / 2 - (stateGetNedToBodyEulers_f()->psi);
+#ifdef USE_SERVO_LIDAR
+ theta = theta - servoLidar.angle * M_PI / 180;
+#endif
+
+ float min_distance = FLT_MAX;
+
+ // Traverse the wall array and store the minimum distance (!= 0)
+ for (uint8_t w = 0; w < wall_system.wall_count; w++) {
+ struct Wall *wall = &wall_system.walls[w];
+ for (uint8_t p = 0; p < wall->count - 1; p++) {
+ struct FloatVect2 p1 = wall->points_ltp[p];
+ struct FloatVect2 p2 = wall->points_ltp[p + 1];
+ float distance = distance_to_wall(theta, &pos, &p1, &p2);
+ if (distance < min_distance && distance > 0) {
+ min_distance = distance;
+ }
+ }
+ }
+
+ // Store that data in the variable tfmini.distance
+ setLidarDistance_f(min_distance);
+}
+
+
+/**
+ * Set the distance of the lidar
+ * This function is used in NPS to set the distance of the lidar
+ */
+void setLidarDistance_f(float distance)
+{
+ if (distance < LIDAR_MIN_RANGE || distance > LIDAR_MAX_RANGE) {
+ distance = 0;
+ }
+ tfmini.distance = distance;
+ tfmini_send_abi();
+}
+
+
+// Send the lidar message (if requested, OBSTACLE_DETECTION)
+void tfmini_send_abi(void)
+{
+#ifndef USE_SERVO_LIDAR
+ //send message (if there is not servo module)
+ AbiSendMsgOBSTACLE_DETECTION(AGL_LIDAR_TFMINI_ID, tfmini.distance, 0, 0);
+#endif
+}