multicopter land detector move ground contact intent to mc_pos_control

src/modules/commander/Commander.cpp

@@ -3102,7 +3102,7 @@ void Commander::battery_status_check()
 	// Compare estimate of RTL time to estimate of remaining flight time
 	if (_vehicle_status_flags.battery_low_remaining_time
 	    && _arm_state_machine.isArmed()
-	    && !_vehicle_land_detected.ground_contact // not in any landing stage
+	    && !(_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed) // not in any landing stage
 	    && !_rtl_time_actions_done
 	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
 	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -212,12 +212,17 @@ 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 trajectory_setpoint;
+		vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
 
-		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
-			// Setpoints can be NAN
-			_in_descend = PX4_ISFINITE(trajectory_setpoint.vz)
-				      && (trajectory_setpoint.vz >= crawl_speed_threshold);
+		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 >= crawl_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;
 		}
 
 		// ground contact requires commanded descent until landed

src/modules/land_detector/MulticopterLandDetector.h

@@ -44,7 +44,6 @@
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/hover_thrust_estimate.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/takeoff_status.h>
@@ -111,9 +110,8 @@ 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_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};

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -360,7 +360,14 @@ void MulticopterPositionControl::Run()
 			}
 		}
 
-		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+				_ground_contact = vehicle_land_detected.ground_contact;
+			}
+		}
 
 		if (_param_mpc_use_hte.get()) {
 			hover_thrust_estimate_s hte;
@@ -372,7 +379,24 @@ void MulticopterPositionControl::Run()
 			}
 		}
 
-		_trajectory_setpoint_sub.update(&_setpoint);
+		if (_trajectory_setpoint_sub.updated()) {
+			trajectory_setpoint_s trajectory_setpoint{};
+
+			if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) {
+				_setpoint.timestamp = trajectory_setpoint.timestamp;
+				_setpoint.x = trajectory_setpoint.position[0];
+				_setpoint.y = trajectory_setpoint.position[1];
+				_setpoint.z = trajectory_setpoint.position[2];
+				_setpoint.vx = trajectory_setpoint.velocity[0];
+				_setpoint.vy = trajectory_setpoint.velocity[1];
+				_setpoint.vz = trajectory_setpoint.velocity[2];
+				_setpoint.acceleration[0] = trajectory_setpoint.acceleration[0];
+				_setpoint.acceleration[1] = trajectory_setpoint.acceleration[1];
+				_setpoint.acceleration[2] = trajectory_setpoint.acceleration[2];
+				_setpoint.yaw = trajectory_setpoint.yaw;
+				_setpoint.yawspeed = trajectory_setpoint.yawspeed;
+			}
+		}
 
 		// adjust existing (or older) setpoint with any EKF reset deltas
 		if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
@@ -471,13 +495,30 @@ void MulticopterPositionControl::Run()
 			}
 
 			// handle smooth takeoff
-			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed,
+			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _landed,
 						    _vehicle_constraints.want_takeoff,
 						    _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample);
 
-			const bool not_taken_off             = (_takeoff.getTakeoffState() < TakeoffState::rampup);
-			const bool flying                    = (_takeoff.getTakeoffState() >= TakeoffState::flight);
-			const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
+			const bool not_taken_off  = (_takeoff.getTakeoffState() < TakeoffState::rampup);
+			const bool flying         = (_takeoff.getTakeoffState() >= TakeoffState::flight);
+			const bool ground_contact = (flying && _ground_contact);
+
+			bool descent_intended = false;
+
+			if (flying && ground_contact) {
+				if (PX4_ISFINITE(_setpoint.z)) {
+					// position setpoint
+					descent_intended = (_setpoint.z > states.position(2));
+
+				} else if (PX4_ISFINITE(_setpoint.vz)) {
+					// velocity setpoint > 90% of MPC_LAND_CRWL
+					descent_intended = (_setpoint.vz > (0.9f * math::max(_param_mpc_land_crwl.get(), 0.1f)));
+
+				} else if (PX4_ISFINITE(_setpoint.acceleration[2])) {
+					// acceleration setpoint
+					descent_intended = (_setpoint.acceleration[2] > 0.f);
+				}
+			}
 
 			if (!flying) {
 				_control.setHoverThrust(_param_mpc_thr_hover.get());
@@ -488,7 +529,7 @@ void MulticopterPositionControl::Run()
 				_setpoint.acceleration[2] = NAN;
 			}
 
-			if (not_taken_off || flying_but_ground_contact) {
+			if (not_taken_off || (flying && ground_contact && descent_intended)) {
 				// we are not flying yet and need to avoid any corrections
 				reset_setpoint_to_nan(_setpoint);
 				_setpoint.timestamp = vehicle_local_position.timestamp_sample;
@@ -567,7 +608,7 @@ void MulticopterPositionControl::Run()
 
 		} else {
 			// an update is necessary here because otherwise the takeoff state doesn't get skipped with non-altitude-controlled modes
-			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true,
+			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _landed, false, 10.f, true,
 						    vehicle_local_position.timestamp_sample);
 		}
 

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -112,6 +112,9 @@ private:
 	vehicle_local_position_setpoint_s _setpoint {};
 	vehicle_control_mode_s _vehicle_control_mode {};
 
+	bool _landed{true};
+	bool _ground_contact{true};
+
 	vehicle_constraints_s _vehicle_constraints {
 		.timestamp = 0,
 		.speed_up = NAN,
@@ -119,14 +122,6 @@ private:
 		.want_takeoff = false,
 	};
 
-	vehicle_land_detected_s _vehicle_land_detected {
-		.timestamp = 0,
-		.freefall = false,
-		.ground_contact = true,
-		.maybe_landed = true,
-		.landed = true,
-	};
-
 	DEFINE_PARAMETERS(
 		// Position Control
 		(ParamFloat<px4::params::MPC_XY_P>)         _param_mpc_xy_p,
@@ -164,6 +159,8 @@ private:
 		(ParamFloat<px4::params::MPC_THR_MAX>)      _param_mpc_thr_max,
 		(ParamFloat<px4::params::MPC_THR_XY_MARG>)  _param_mpc_thr_xy_marg,
 
+		(ParamFloat<px4::params::MPC_LAND_CRWL>)    _param_mpc_land_crwl,
+
 		(ParamFloat<px4::params::SYS_VEHICLE_RESP>) _param_sys_vehicle_resp,
 		(ParamFloat<px4::params::MPC_ACC_HOR>)      _param_mpc_acc_hor,
 		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,