diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 3acc4d03a3..2c3d43dc73 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -227,14 +227,14 @@ void FlightTaskAuto::_prepareLandSetpoints() _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint // Slow down automatic descend close to ground - float speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); + float vertical_speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) { - speed = _param_mpc_land_crawl_speed.get(); + vertical_speed = _param_mpc_land_crawl_speed.get(); } if (_type_previous != WaypointType::land) { @@ -247,7 +247,7 @@ void FlightTaskAuto::_prepareLandSetpoints() // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) { // Stick full up -1 -> stop, stick full down 1 -> double the speed - speed *= (1 + _sticks.getPositionExpo()(2)); + vertical_speed *= (1 + _sticks.getPositionExpo()(2)); _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); @@ -269,7 +269,7 @@ void FlightTaskAuto::_prepareLandSetpoints() _position_setpoint = _land_position; // The last element of the land position has to stay NAN _yaw_setpoint = _land_heading; - _velocity_setpoint(2) = speed; + _velocity_setpoint(2) = vertical_speed; _gear.landing_gear = landing_gear_s::GEAR_DOWN; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 95cdd652b1..9d90d61de2 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -80,6 +80,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); + _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); } void MulticopterLandDetector::_update_topics() @@ -157,13 +158,13 @@ bool MulticopterLandDetector::_get_ground_contact_state() const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); // land speed threshold, 90% of MPC_LAND_SPEED - const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); + const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f); if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication. - float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); + float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { // Widen acceptance thresholds for landed state right after arming @@ -215,7 +216,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) - && (trajectory_setpoint.vz >= land_speed_threshold); + && (trajectory_setpoint.vz >= crawl_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 33f4fd2729..e11dd4e555 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -95,6 +95,7 @@ private: param_t hoverThrottle; param_t minManThrottle; param_t landSpeed; + param_t crawlSpeed; param_t useHoverThrustEstimate; } _paramHandle{}; @@ -103,6 +104,7 @@ private: float hoverThrottle; float minManThrottle; float landSpeed; + float crawlSpeed; bool useHoverThrustEstimate; } _params{};