FW LandDetector: disregard horizontal velocity if local_position.v_xy_valid is false

[mahimayoga]

Solved Problem

Horizontal velocity considered in land detector for fixed wings even when invalid.

Solution

• Add condition to disregard horizontal velocity when vehicle_local_position.v_xy_valid is false.
• Add additional rotational velocity condition for landed state.

Changelog Entry

For release notes:

Feature/Bugfix: Add condition to disregard horizontal velocity when vehicle_local_position.v_xy_valid is false in land detector for fixed wings. Add extra land condition based on rotational velocity. 

Test coverage

• Simulation/hardware testing logs: to be tested

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 248 byte (0.01 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +248  +0.0%    +248    .text
   +14%    +144   +14%    +144    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +0.2%     +40  +0.2%     +40    ../../src/lib/parameters/parameters.cpp
  +0.0%     +40  +0.0%     +40    ROMFS/nsh_romfsimg.c
  +0.0%     +21  +0.0%     +21    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +20  [ = ]       0    .debug_frame
+0.0% +3.30Ki  [ = ]       0    .debug_info
  +0.0%      +8  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/rc/crsf_rc/CrsfRc.cpp
 -100.0% +3.15Ki  [ = ]       0    [171 Others]
+0.0%    +312  [ = ]       0    .debug_line
  +0.2%     +37  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +7.0%    +301  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.0% +1.07Ki  [ = ]       0    .debug_loc
  -0.1%     -15  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.0%     +30  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  -0.0%     -13  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.1%     +15  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  -0.1%     -15  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +0.1%     +16  [ = ]       0    ../../src/lib/mixer_module/mixer_module.cpp
  -0.0%      -2  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -0.2%     -15  [ = ]       0    ../../src/lib/rtl/rtl_time_estimator.cpp
  +0.0%     +15  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/commander/failure_detector/FailureDetector.cpp
  -0.6%     -48  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/gyro_calibration/GyroCalibration.cpp
   +26% +1.08Ki  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.0%      +3  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
 -100.0%     +15  [ = ]       0    [5 Others]
+0.0%    +208  [ = ]       0    .debug_ranges
  +0.4%     +56  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
   +14%    +160  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
+0.0%    +996  [ = ]       0    .debug_str
  +0.0%     +14  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  +0.0%     +11  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
   +13%    +129  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +5.4%    +841  [ = ]       0    ../../src/modules/land_detector/land_detector_main.cpp
  +0.0%      +1  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
-1.5%    -248  [ = ]       0    [Unmapped]
+0.0% +5.92Ki  +0.0%    +248    TOTAL

px4_fmu-v6x [Total VM Diff: 200 byte (0.01 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +200  +0.0%    +200    .text
   +14%    +144   +14%    +144    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +0.0%     +28  +0.0%     +28    ROMFS/nsh_romfsimg.c
  +0.0%     +20  +0.0%     +20    [section .text]
  +0.0%      +8  +0.0%      +8    ../../src/lib/parameters/parameters.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +8  [ = ]       0    .debug_frame
+0.0% +3.22Ki  [ = ]       0    .debug_info
  +0.0%      +8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/rc_input/RCInput.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/uavcan/actuators/esc.cpp
 -100.0% +3.07Ki  [ = ]       0    [160 Others]
+0.0%    +276  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +7.0%    +301  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
+0.0% +1.13Ki  [ = ]       0    .debug_loc
  +0.1%     +15  [ = ]       0    ../../src/drivers/uavcan/remoteid.cpp
  +0.0%     +14  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/commander/failure_detector/FailureDetector.cpp
  +0.6%     +48  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
   +26% +1.08Ki  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.0%     -15  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +0.2%     +30  [ = ]       0    ../../src/modules/sensors/voted_sensors_update.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/temperature_compensation/TemperatureCompensationModule.cpp
  +0.0%     +28  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.0%     -15  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%    +153  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
   +14%    +160  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +984  [ = ]       0    .debug_str
  +0.0%     +14  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
   +13%    +129  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +5.3%    +841  [ = ]       0    ../../src/modules/land_detector/land_detector_main.cpp
-0.3%    -200  [ = ]       0    [Unmapped]
+0.0% +5.79Ki  +0.0%    +200    TOTAL

Updated: 2025-01-08T16:07:29

[sfuhrer]

With that change we stay on the conservative side: It makes triggering a false-positive "landing detected* in air very unlikely, with the cost of more false positive "takeoff detected" while still on the ground due to the very tight rotational movement threshold. Given the much higher severity of a false-positive in air I'm okay with that.
A slightly less drastic change would be to only consider the rotational movement check if the velocity (airspeed and groundspeed) are invalid, or work with different thresholds depending on velocity validity. But that in turn introduces more logic complexity, the system gets much harder to tune and debug.
As a next step we could give the FW land detector a bigger overhaul. I want to link it more directly with the FW launch detection and