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};