Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Draft
wants to merge 7 commits into
base: main
Choose a base branch
from
Draft

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

wants to merge 7 commits into from
+42 −9

Conversation

mahimayoga
Copy link
Contributor

@mahimayoga mahimayoga commented Dec 19, 2024 •
edited
Loading

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 GitHub Actions
Copy link

github-actions bot commented Dec 20, 2024 •
edited
Loading

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 200 byte (0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +200  +0.0%    +200    .text
   +13%    +136   +13%    +136    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +0.2%     +40  +0.2%     +40    ../../src/lib/parameters/parameters.cpp
  +0.0%     +15  +0.0%     +15    ROMFS/nsh_romfsimg.c
  +0.0%      +9  +0.0%      +9    [section .text]
+0.0%    +106  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +1.8%     +50  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +20  [ = ]       0    .debug_frame
+0.0% +5.35Ki  [ = ]       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% +5.20Ki  [ = ]       0    [171 Others]
+0.0%    +341  [ = ]       0    .debug_line
  +0.2%     +37  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +7.7%    +331  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.2%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%    +883  [ = ]       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
   +22%    +960  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.1%     -16  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  -0.5%     -15  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  -0.4%     -15  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.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%    +201  [ = ]       0    .debug_ranges
  +0.4%     +56  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
   +14%    +152  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +1.6%      +1  [ = ]       0    task/task_cancelpt.c
+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.2%    -200  [ = ]       0    [Unmapped]
+0.0% +7.83Ki  +0.0%    +200    TOTAL
px4_fmu-v6x [Total VM Diff: 184 byte (0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +184  +0.0%    +184    .text
   +13%    +136   +13%    +136    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +0.0%     +24  +0.0%     +24    [section .text]
  +0.0%     +13  +0.0%     +13    ROMFS/nsh_romfsimg.c
  +0.0%      +8  +0.0%      +8    ../../src/lib/parameters/parameters.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%    +106  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +1.8%     +50  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +8  [ = ]       0    .debug_frame
+0.0% +5.27Ki  [ = ]       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% +5.11Ki  [ = ]       0    [160 Others]
+0.0%    +305  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +7.7%    +331  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.0%   +1008  [ = ]       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
   +22%    +960  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  -0.0%     -15  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.1%     +16  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.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%    +144  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
   +14%    +152  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
+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.2%    -184  [ = ]       0    [Unmapped]
+0.0% +7.76Ki  +0.0%    +184    TOTAL

Updated: 2025-01-07T15:16:07

mahimayoga
mahimayoga commented Jan 6, 2025
View reviewed changes
src/modules/land_detector/FixedwingLandDetector.cpp
@@ -105,15 +114,28 @@ bool FixedwingLandDetector::_get_landed_state()
const float acc_hor = matrix::Vector2f(_acceleration).norm();
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;

// Check for angular velocity (disregard yaw)
const float rot_vel_hor = _angular_velocity.xy().norm();
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Considering only rotations around x & y to be consistent with MC landing condition, but can be extended to all three axes if it makes sense in FW context.

mahimayoga
mahimayoga commented Jan 7, 2025
View reviewed changes
src/modules/land_detector/land_detector_params_fw.c
@@ -112,4 +112,4 @@ PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f);
* @decimal 1
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.06f);
PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Threshold based on logs of 8 FW flights. Highest value that results in 0 false positives.

If it's too low, could consider increasing default hysteresis time to transition into landed state, because false positives for higher thresholds are mostly non-consecutive - but I don't know if it makes sense to do that.

@mahimayoga mahimayoga requested a review from sfuhrer January 7, 2025 15:17
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@sfuhrer sfuhrer Awaiting requested review from sfuhrer

At least 1 approving review is required to merge this pull request.

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@mahimayoga @sfuhrer