Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-19 18:42:46 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Add Bayesian Obstacle Grid Mapping with TFmini Lidar Support and Simulation (#3510)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source
This commit is contained in:
Alejandro Rochas Fernández
2025-08-07 23:17:19 +02:00
committed by GitHub
parent b706cebb5a
commit 4e7259a17f
18 changed files with 1646 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+1
View File
@@ -265,6 +265,7 @@
<field name="stamp" type="uint32_t" unit="us"/>
<field name="relpos" type="struct RelPosNED *"/>
</message>
</msg_class>
</protocol>
conf/airframes/UCM/conf.xml
+13
View File
@@ -0,0 +1,13 @@
<conf>
<aircraft
name="Rover_Lidar"
ac_id="10"
airframe="airframes/UCM/rover_lidar.xml"
radio="radios/UCM/T16SZ_SBUS_rover.xml"
telemetry="telemetry/default_lidar_telemetry.xml"
flight_plan="flight_plans/UCM/mission_lidar.xml"
settings=""
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/lidar_tfmini.xml modules/nav_rover_base.xml modules/obstacle_rover.xml~Obstacle Grid~ modules/servo_lidar.xml"
gui_color="#35008400e400"
/>
</conf>
conf/airframes/UCM/rover_lidar.xml
+231
View File
@@ -0,0 +1,231 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="boat">
<firmware name="rover">
<autopilot name="rover_steering"/>
<target name="ap" board="matek_f405_wing_v1">
<configure name="PERIODIC_FREQUENCY" value="100"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART2"/>
</module>
</target>
<target name="nps" board="pc">
<module name="radio_control" type="ppm"/>
<module name="fdm" type="rover"/>
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
<define name="NPS_BYPASS_INS" value="TRUE"/>
<module name="telemetry_nps" dir="datalink" />
</target>
<!-- Same board module as matek_f405 but with another IMU -->
<module name="board" type="matek_f405_wing"/> <!-- IMU included -->
<module name="actuators" type="pwm"/>
<!--F405-WSE GPS and Compass connector are mapped to ports UART4 and I2C2 -->
<module name="gps" type="ublox">
<configure name="GPS_PORT" value="UART3"/>
</module>
<module name="gps_ubx_ucenter"/>
<module name="ins"/>
<module name="ahrs" type="float_dcm">
<define name="AHRS_FLOAT_MIN_SPEED_GPS_COURSE" value="0.1"/>
</module>
<module name="telemetry" type="xbee_api">
<configure name="MODEM_PORT" value="UART1"/>
</module>
<!-- Compass ublox m8n -->
<!--F405-WSE GPS and Compass connector are mapped to ports UART4 and I2C2 -->
<module name="mag_hmc58xx">
<configure name="MAG_HMC58XX_I2C_DEV" value="I2C2"/>
<define name="MODULE_HMC58XX_SYNC_SEND" value="TRUE"/>
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
</module>
<!-- Our modules -->
<!-- Lidar related modules-->
<module name="lidar_tfmini">
<configure name="TFMINI_PORT" value="UART6"/>
<configure name="TFMINI_BAUD" value="B115200"/>
<configure name="LIDAR_OFFSET" value="0.20"/>
<configure name="LIDAR_HEIGHT" value="0.24"/>
<configure name="USE_TFMINI_AGL" value="TRUE"/>
<configure name="TFMINI_COMPENSATE_ROTATION" value="TRUE"/>
<configure name="TFMINI_ROVER" value="TRUE"/>
</module>
<module name="lidar_correction"/>
<module name="servo_lidar"/>
<!-- Navegation -->
<module name="nav" type="rover_base"/>
<module name="gvf" type="classic">
<define name="GVF_OCAML_GCS" value="FALSE"/>
</module>
<module name="gvf_parametric">
<define name="GVF_PARAMETRIC_2D_BEZIER_N_SEG" value="4"/>
</module>
<!-- Obstacle grid -->
<module name="obstacle_rover">
<define name="N_COL_GRID" value="100"/>
<define name="N_ROW_GRID" value="100"/>
</module>
<module name="guidance" type="rover_steering">
<define name="MAX_DELTA" value="15.0"/>
<define name="DRIVE_SHAFT_DISTANCE" value="0.25"/>
<define name="SR_MEASURED_KF" value="1400.0"/>
<define name="MIN_CMD_SHUT" value="3800"/>
<define name="MAX_CMD_SHUT" value="1350"/>
<define name="MOV_AVG_M" value="200"/>
</module>
</firmware>
<!-- COMMANDS SECTION ..................................................................... -->
<servos>
<!-- "no" 1 and 2 but no 0 because our board don't have S0 actuator entry...
On the board is the S3 and S4 -->
<servo name="MOTOR_THROTTLE" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="MOTOR_STEERING" no="3" min="1000" neutral="1500" max="2000"/>
<servo name="MOTOR_LIDAR" no="1" min="500" neutral="1500" max="2500"/>
</servos>
<!-- Low level commands (PWM signal) -->
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="STEERING" failsafe_value="0"/>
<axis name="SERVO" failsafe_value="0"/>
</commands>
<!-- When SetCommandsFromRC -->
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="STEERING" value="@ROLL"/>
</rc_commands>
<!-- When SetActuatorsFromCommands-->
<command_laws>
<set servo="MOTOR_THROTTLE" value="@THROTTLE"/>
<set servo="MOTOR_STEERING" value="@STEERING"/>
<set servo="MOTOR_LIDAR" value="@SERVO"/>
</command_laws>
<!-- SECTIONS: General config ............................................................. -->
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- ....CALIBRATION 2023/12/04 .................................................... -->
<define name="ACCEL_X_NEUTRAL" value="34"/>
<define name="ACCEL_Y_NEUTRAL" value="13"/>
<define name="ACCEL_Z_NEUTRAL" value="-217"/>
<define name="ACCEL_X_SENS" value="4.910468072567044" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.87915023807254" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.787109540792222" integer="16"/>
<!-- ......CALIBRATION 2022/07/30..................................................... -->
<define name="MAG_X_NEUTRAL" value="256"/>
<define name="MAG_Y_NEUTRAL" value="-412"/>
<define name="MAG_Z_NEUTRAL" value="71"/>
<define name="MAG_X_SENS" value="3.707420822119781" integer="16"/>
<define name="MAG_Y_SENS" value="4.148937385955166" integer="16"/>
<define name="MAG_Z_SENS" value="3.242411432686237" integer="16"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix (i.e NPS), on 3D fix is update by geo_mag module if loaded -->
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<define name="USE_GPS" value="TRUE"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_CRUISER"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <!-- Ignoring radio_control.values -->
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="6.35" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="6.70" unit="V"/>
<define name="LOW_BAT_LEVEL" value="7.05" unit="V"/>
<define name="MAX_BAT_LEVEL" value="8.44" unit="V"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="1800" unit="mA"/>
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" unit="mA"/>
</section>
<section name="GCS">
<define name="AC_ICON" value="rover"/>
</section>
<!-- Maps -->
<section name="OBSTACLE_CONFIG" prefix="OBSTACLE_">
<define name="WALLS" type="array">
<!-- Padel South Wall -->
<field type="struct">
<field name="count" value="2"/>
<field name="points" type="struct">
<field value="{40.4512650, -3.7291591, 650.0}"/>
<field value="{40.4512050, -3.7291535, 650.0}"/>
</field>
</field>
<!-- Padel West Wall -->
<field type="struct">
<field name="count" value="3"/>
<field name="points" type="struct">
<field value="{40.4512037, -3.7291532, 650.0}"/>
<field value="{40.4512084, -3.7291015, 650.0}"/>
<field value="{40.4512295, -3.7289073, 650.0}"/>
</field>
</field>
<!-- Tower -->
<field type="struct">
<field name="count" value="4"/>
<field name="points" type="struct">
<field value="{40.4513016, -3.7289494, 650.0}"/>
<field value="{40.4513006, -3.7289645, 650.0}"/>
<field value="{40.4513107, -3.7289655, 650.0}"/>
<field value="{40.4513120, -3.7289500, 650.0}"/>
</field>
</field>
<!-- Grades -->
<field type="struct">
<field name="count" value="2"/>
<field name="points" type="struct">
<field value="{40.451918, -3.729198, 650.0}"/>
<field value="{40.452028, -3.728153, 650.0}"/>
</field>
</field>
</define>
</section>
</airframe>
conf/flight_plans/UCM/mission_lidar.xml
+92
View File
@@ -0,0 +1,92 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="660" ground_alt="650" lat0="40.4512539" lon0="-3.7289867" max_dist_from_home="2500" name="Rover Steering" security_height="0.3">
<header>
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint lat="40.4515279" lon="-3.7289089" name="STDBY" />
<waypoint lat="40.4512609" lon="-3.7290940" name="ELLIPSE" />
<waypoint lat="40.4516215" lon="-3.7290142" name="P1" />
<waypoint lat="40.4517065" lon="-3.7282202" name="P2"/>
<waypoint name="_N1" x="90" y="90"/>
<waypoint name="_N2" x="90" y="-90"/>
<waypoint name="_N3" x="-90" y="-90"/>
<waypoint name="_N4" x="-90" y="90"/>
<waypoint name="_G1" x="-50" y="-50"/>
<waypoint name="_G2" x="50" y="-50"/>
<waypoint name="_G3" x="50" y="50"/>
<waypoint name="_G4" x="-50" y="50"/>
</waypoints>
<sectors>
<sector color="red" name="Net">
<corner name="_N1"/>
<corner name="_N2"/>
<corner name="_N3"/>
<corner name="_N4"/>
</sector>
<sector color="blue" name="Grid">
<corner name="_G1"/>
<corner name="_G2"/>
<corner name="_G3"/>
<corner name="_G4"/>
</sector>
</sectors>
<variables>
<variable init="5.0" max="10.0" min="1.0" step="1.0" var="a_stb"/>
<variable init="5.0" max="10.0" min="1.0" step="1.0" var="b_stb"/>
</variables>
<blocks>
<block name="Wait GPS" strip_icon="gps.png">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init" strip_icon="googleearth.png">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
<call_once fun="init_grid_4(WP__G1, WP__G2, WP__G3, WP__G4)"/> <!-- Init obstacle grid -->
</block>
<block name="Start Engine" strip_icon="on.png">
<while cond="LessThan(NavBlockTime(), 1)"/>
<call_once fun="autopilot_set_motors_on(TRUE)"/>
</block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<call fun="nav_gvf_ellipse_wp(WP_ELLIPSE, a_stb, b_stb, gvf_ellipse_par.alpha)"/>
</block>
<block name="line_to_HOME" strip_icon="home_drop.png">
<call fun="nav_gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
</block>
<block name="ellipse_wp" strip_icon="oval.png">
<call fun="nav_gvf_ellipse_wp(WP_ELLIPSE, a_stb, b_stb, gvf_ellipse_par.alpha)"/>
</block>
<block name="nav_gvf_ellipse_XY" strip_icon="oval.png">
<call fun="nav_gvf_ellipse_XY(10.1, 10.1, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>
<block name="line_P1_P2" strip_icon="line.png">
<call fun="nav_gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
</block>
<block name="segment_turn_P1_P2" strip_icon="line.png">
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="sin_p1_p2" strip_icon="line_drop.png">
<call fun="nav_gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
</block>
</blocks>
</flight_plan>
conf/modules/lidar_correction.xml
+26
View File
@@ -0,0 +1,26 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="lidar_correction" dir="lidar/slam">
<doc>
<description>
LIDAR correction for SLAM applications
Includes functions used to simulate the LIDAR data
(Optional module for advanced LIDAR processing)
</description>
<configure name="USE_LIDAR_CORRECTION" value="true" description="Enable LIDAR correction module"/>
</doc>
<header>
<file name="lidar_correction.h"/>
</header>
<init fun="init_walls()"/>
<makefile target="ap|nps">
<configure name="USE_LIDAR_CORRECTION" default="true"/>
<define name="USE_LIDAR_CORRECTION" value="$(USE_LIDAR_CORRECTION)"/>
<configure name="OBSTACLES_CONFIG" default="ucm_football"/>
<define name="OBSTACLES_CONFIG_FILE" value="modules/lidar/maps/$(OBSTACLES_CONFIG).h"/>
<file name="lidar_correction.c"/>
</makefile>
</module>
conf/modules/lidar_tfmini.xml
+39 -8
View File
@@ -3,19 +3,26 @@
<module name="lidar_tfmini" dir="lidar">
<doc>
<description>
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)
</description>
<configure name="TFMINI_PORT" value="UART3" description="UART device to use for lidar"/>
<configure name="TFMINI_BAUD" value="B115200" description="baudrate of the TFMini UART port"/>
<configure name="USE_TFMINI_AGL" value="0" description="use this lidar for AGL measurements"/>
<configure name="TFMINI_COMPENSATE_ROTATION" value="1" description="compensate AGL measurements for body rotation"/>
<configure name="USE_TFMINI_AGL" value="true" description="use this lidar for AGL measurements"/>
<configure name="TFMINI_COMPENSATE_ROTATION" value="0" description="compensate AGL measurements for body rotation"/>
<configure name="TFMINI_ROVER" value="0" description="is this lidar mounted on a rover? Change the way to calculate the compensation"/>
<configure name="LIDAR_OFFSET" value="0.0" description="Horizontal offset from vehicle center to LIDAR [m]"/>
<configure name="LIDAR_HEIGHT" value="0.0" description="Vertical height from ground to LIDAR [m]"/>
<configure name="LIDAR_MIN_RANGE" value="0.1" description="minimum range of the lidar in meters (see datasheet)"/>
<configure name="LIDAR_MAX_RANGE" value="12.0" description="maximum range of the lidar in meters (see datasheet)"/>
</doc>
<settings>
<dl_settings NAME="Lidar TFMini">
<dl_settings NAME="Lidar">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.compensate_rotation" shortname="derotate_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.update_agl" shortname="update_agl"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.compensate_rotation" shortname="compensate_rotation"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.update_agl" shortname="update_agl"/>
</dl_settings>
</dl_settings>
</settings>
@@ -29,7 +36,9 @@
</header>
<init fun="tfmini_init()"/>
<event fun="tfmini_event()"/>
<makefile>
<makefile target="ap">
<!-- Configure default UART port and baudrate -->
<configure name="TFMINI_PORT" default="UART3" case="upper|lower"/>
<configure name="TFMINI_BAUD" default="B115200"/>
@@ -39,14 +48,36 @@
<define name="$(TFMINI_PORT_UPPER)_BAUD" value="$(TFMINI_BAUD)"/>
<define name="TFMINI_PORT" value="$(TFMINI_PORT_LOWER)"/>
<configure name="USE_TFMINI_AGL" default="0"/>
<!-- Compensation constants -->
<configure name="LIDAR_OFFSET" default="0.0"/>
<configure name="LIDAR_HEIGHT" default="0.0"/>
<define name="LIDAR_OFFSET" value="$(LIDAR_OFFSET)"/>
<define name="LIDAR_HEIGHT" value="$(LIDAR_HEIGHT)"/>
<configure name="USE_TFMINI_AGL" default="true"/>
<configure name="TFMINI_COMPENSATE_ROTATION" default="0"/>
<configure name="TFMINI_ROVER" default="0"/>
<define name="USE_TFMINI_AGL" value="$(USE_TFMINI_AGL)"/>
<define name="TFMINI_COMPENSATE_ROTATION" value="$(TFMINI_COMPENSATE_ROTATION)"/>
<define name="TFMINI_ROVER" value="$(TFMINI_ROVER)"/>
<file name="tfmini.c"/>
</makefile>
<makefile target="nps">
<define name="USE_SONAR" value="1"/><!-- in NPS use a virtual sonar to simulate lidar measurements -->
<!-- Configure default UART port and baudrate -->
<configure name="TFMINI_PORT" default="UART3" case="upper|lower"/>
<configure name="TFMINI_BAUD" default="B115200"/>
<!-- Enable UART and set baudrate -->
<define name="USE_$(TFMINI_PORT_UPPER)"/>
<define name="$(TFMINI_PORT_UPPER)_BAUD" value="$(TFMINI_BAUD)"/>
<define name="TFMINI_PORT" value="$(TFMINI_PORT_LOWER)"/>
<configure name="LIDAR_MIN_RANGE" default="0.1"/>
<configure name="LIDAR_MAX_RANGE" default="12.0"/>
<define name="LIDAR_MIN_RANGE" value="$(LIDAR_MIN_RANGE)"/>
<define name="LIDAR_MAX_RANGE" value="$(LIDAR_MAX_RANGE)"/>
<file name="tfmini_nps.c"/>
</makefile>
</module>
conf/modules/obstacle_rover.xml
+36
View File
@@ -0,0 +1,36 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="obstacle_rover" dir="firmwares/rover/obstacles">
<doc>
<description>
Obstacle rover grid
</description>
<define name="USE_GRID" value="TRUE|FALSE" description="Use it to enable this module"/>
</doc>
<settings name="Obstacle Grid">
<dl_settings>
<dl_settings NAME="Obstacle Grid">
<dl_setting MAX="1" MIN="0.5" STEP="0.05" VAR="obstacle_grid.map.threshold" shortname="threshold"/>
<dl_setting MAX="1" MIN="0.5" STEP="0.05" VAR="obstacle_grid.map.occ" shortname="occupied"/>
<dl_setting MAX="0.5" MIN="0" STEP="0.05" VAR="obstacle_grid.map.free" shortname="free"/>
<dl_setting MAX="10" MIN="1" STEP="1" VAR="grid_block_size" shortname="radius"/>
<dl_setting MAX="20" MIN="0" STEP="1" VAR="obstacle_grid.map.decay" shortname="decay"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rover_obstacles.h"/>
</header>
<periodic fun="decay_map()" freq="100" autorun="TRUE"/>
<makefile firmware="rover">
<file name="rover_obstacles.c" dir="$(SRC_FIRMWARE)/obstacles"/>
<define name="USE_GRID" value="TRUE"/>
</makefile>
</module>
conf/modules/servo_lidar.xml
+33
View File
@@ -0,0 +1,33 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="servo_lidar" dir="lidar/slam">
<doc>
<description>
Servo-mounted LIDAR scanning system
(Uses tfmini driver and provides scanning functionality)
</description>
<configure name="USE_SERVO_LIDAR" value="true|false" description="To enable the servo module"/>
</doc>
<settings>
<dl_settings NAME="Servo LIDAR">
<dl_settings NAME="Servo LIDAR">
<dl_setting var="servoLidar.enabled" min="0" step="1" max="1" shortname="Enable" values="Disabled|Enabled"/>
<dl_setting var="servoLidar.speed" min="1" step="1" max="20" shortname="Speed"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="servo_lidar.h"/>
</header>
<init fun="servoLidar_init()"/>
<periodic fun="servoLidar_periodic()" freq="100" autorun="TRUE"/>
<makefile target="ap|nps">
<configure name="USE_SERVO_LIDAR" default="TRUE"/>
<define name="USE_SERVO_LIDAR" value="$(USE_SERVO_LIDAR)"/>
<file name="servo_lidar.c"/>
</makefile>
</module>
conf/telemetry/default_lidar_telemetry.xml
+72
View File
@@ -0,0 +1,72 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="INFO_MSG" period="1.0"/>
<message name="ALIVE" period="2.7"/>
<message name="ATTITUDE" period="0.5"/>
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="ENERGY" period="8.5"/>
<message name="GPS_INT" period="0.35"/>
<message name="GPS_SOL" period="2.0"/>
<message name="INS" period="0.2"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_FP" period="0.35"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="2.3"/>
<message name="GVF" period="4.0"/>
<message name="GVF_PARAMETRIC" period="2.0"/>
<message name="LIDAR" period="0.2"/>
<message name="SLAM" period="0.2"/>
<message name="GRID_INIT" period="15.0"/>
</mode>
<mode name="low" key_press="l">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="ROVER_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="1.0"/>
<message name="ALIVE" period="2.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="INS_REF" period="5.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
</mode>
<mode name="rc">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROVER_STATUS" period="1.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROVER_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
</process>
</telemetry>
sw/airborne/firmwares/rover/obstacles/rover_obstacles.c
+412
View File
@@ -0,0 +1,412 @@
/*
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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);
}
sw/airborne/firmwares/rover/obstacles/rover_obstacles.h
+88
View File
@@ -0,0 +1,88 @@
/*
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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
sw/airborne/modules/lidar/slam/lidar_correction.c
+184
View File
@@ -0,0 +1,184 @@
/*
* Copyright (C) 2025 Alejandro Rochas Fernández <alrochas@ucm.es>
*
* 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 <math.h>
#include <float.h>
#include <state.h>
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;
}
sw/airborne/modules/lidar/slam/lidar_correction.h
+72
View File
@@ -0,0 +1,72 @@
/*
* Copyright (C) 2025 Alejandro Rochas Fernández <alrochas@ucm.es>
*
* 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
sw/airborne/modules/lidar/slam/servo_lidar.c
+85
View File
@@ -0,0 +1,85 @@
/*
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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
sw/airborne/modules/lidar/slam/servo_lidar.h
+50
View File
@@ -0,0 +1,50 @@
/*
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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
sw/airborne/modules/lidar/tfmini.c
+61 -12
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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
}
sw/airborne/modules/lidar/tfmini.h
+11 -1
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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 */
sw/airborne/modules/lidar/tfmini_nps.c
+140
View File
@@ -0,0 +1,140 @@
/*
* Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
*
* 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
}