From c28533677dbcbdc772524c3126921a6d322dd29b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 22 Apr 2021 15:53:48 +0200 Subject: [PATCH] MulticopterLandDetector: use setpoint generation to infer decend intent For any normal use case where a downwards velocity setpoint is set this works exactly the same as before. E.g. autonomous landing, landing in Altitude or Position mode The advantage is that the very common case where a vehicle tries to hold a constant altitude but fails to do so e.g. during a hard brake with too much lift the resulting downwards velocity was interpreted as descend intent and since the vehicle already struggled to hold altitude with low thrust and was not moving fast anymore because it was braking this lead to a lot more false positives on certain vehicle types. The disadvantage is that not setting a downwards velocity setpoint but just moving the position setpoint into the ground does not result in land detection anymore. We do not use this method of landing anymore for quite a while. It's not recommended and I wonder if there's some rare use case like offboard where this is done. We could add an additional case for the specific case to land with a position setpoint only. --- .../land_detector/MulticopterLandDetector.cpp | 15 +++++---------- .../land_detector/MulticopterLandDetector.h | 2 +- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 0aade86b48..15921ef30d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -210,17 +210,12 @@ bool MulticopterLandDetector::_get_ground_contact_state() // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground if (_flag_control_climb_rate_enabled) { - vehicle_local_position_setpoint_s vehicle_local_position_setpoint; + vehicle_local_position_setpoint_s trajectory_setpoint; - if (_vehicle_local_position_setpoint_sub.update(&vehicle_local_position_setpoint)) { - // setpoints can briefly be NAN to signal resets, TODO: fix in multicopter position controller - const bool descend_vel_sp = PX4_ISFINITE(vehicle_local_position_setpoint.vz) - && (vehicle_local_position_setpoint.vz >= land_speed_threshold); - - const bool descend_acc_sp = PX4_ISFINITE(vehicle_local_position_setpoint.acceleration[2]) - && (vehicle_local_position_setpoint.acceleration[2] >= 100.f); - - _in_descend = descend_vel_sp || descend_acc_sp; + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + // Setpoints can be NAN + _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) + && (trajectory_setpoint.vz >= land_speed_threshold); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index e6ed259715..4045d6ef13 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -112,9 +112,9 @@ private: uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; hrt_abstime _hover_thrust_estimate_last_valid{0};