mc_pos_control: require current trajectory setpoint to run controller

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -319,9 +319,21 @@ void MulticopterPositionControl::Run()
 
 		// set _dt in controllib Block for BlockDerivative
 		setDt(dt);
-		_in_failsafe = false;
 
-		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
+		if (_vehicle_control_mode_sub.updated()) {
+			const bool position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled;
+
+			if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) {
+				if (!position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled) {
+					_time_position_control_enabled = _vehicle_control_mode.timestamp;
+
+				} else if (position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
+					// clear existing setpoint when controller is no longer active
+					_setpoint = {};
+				}
+			}
+		}
+
 		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 
 		if (_param_mpc_use_hte.get()) {
@@ -334,14 +346,10 @@ void MulticopterPositionControl::Run()
 			}
 		}
 
-		PositionControlStates states{set_vehicle_states(local_pos)};
-
-		if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
-
 		const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
 
 		// adjust existing (or older) setpoint with any EKF reset deltas
-			if (_setpoint.timestamp < local_pos.timestamp) {
+		if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < local_pos.timestamp)) {
 			if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
 				_setpoint.vx += local_pos.delta_vxy[0];
 				_setpoint.vy += local_pos.delta_vxy[1];
@@ -365,6 +373,40 @@ void MulticopterPositionControl::Run()
 			}
 		}
 
+		if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
+			_vel_x_deriv.reset();
+			_vel_y_deriv.reset();
+		}
+
+		if (local_pos.vz_reset_counter != _vz_reset_counter) {
+			_vel_z_deriv.reset();
+		}
+
+		// save latest reset counters
+		_vxy_reset_counter = local_pos.vxy_reset_counter;
+		_vz_reset_counter = local_pos.vz_reset_counter;
+		_xy_reset_counter = local_pos.xy_reset_counter;
+		_z_reset_counter = local_pos.z_reset_counter;
+		_heading_reset_counter = local_pos.heading_reset_counter;
+
+
+		PositionControlStates states{set_vehicle_states(local_pos)};
+
+
+		if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
+			// set failsafe setpoint if there hasn't been a new
+			// trajectory setpoint since position control started
+			if ((_setpoint.timestamp < _time_position_control_enabled)
+			    && (time_stamp_now > _time_position_control_enabled)) {
+
+				failsafe(time_stamp_now, _setpoint, states);
+				_setpoint.timestamp = local_pos.timestamp;
+			}
+		}
+
+		if (_vehicle_control_mode.flag_multicopter_position_control_enabled
+		    && (_setpoint.timestamp >= _time_position_control_enabled)) {
+
 			// update vehicle constraints and handle smooth takeoff
 			_vehicle_constraints_sub.update(&_vehicle_constraints);
 
@@ -377,7 +419,7 @@ void MulticopterPositionControl::Run()
 			if (_vehicle_control_mode.flag_control_offboard_enabled) {
 
 				bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed
-						    && hrt_elapsed_time(&_setpoint.timestamp) < 1_s;
+						    && (time_stamp_now < _setpoint.timestamp + 1_s);
 
 				if (want_takeoff && PX4_ISFINITE(_setpoint.z)
 				    && (_setpoint.z < states.position(2))) {
@@ -474,17 +516,9 @@ void MulticopterPositionControl::Run()
 
 			} else {
 				// Failsafe
-				//  do not warn while we are disarmed, as we might not have valid setpoints yet
-				const bool warn_failsafe = ((time_stamp_now - _last_warn) > 2_s) && _vehicle_control_mode.flag_armed;
-
-				if (warn_failsafe) {
-					PX4_WARN("invalid setpoints");
-					_last_warn = time_stamp_now;
-				}
-
 				vehicle_local_position_setpoint_s failsafe_setpoint{};
 
-				failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe);
+				failsafe(time_stamp_now, failsafe_setpoint, states);
 
 				// reset constraints
 				_vehicle_constraints = {0, NAN, NAN, false, {}};
@@ -524,21 +558,22 @@ void MulticopterPositionControl::Run()
 			_takeoff_status_pub.get().timestamp = hrt_absolute_time();
 			_takeoff_status_pub.update();
 		}
-
-		// save latest reset counters
-		_vxy_reset_counter = local_pos.vxy_reset_counter;
-		_vz_reset_counter = local_pos.vz_reset_counter;
-		_xy_reset_counter = local_pos.xy_reset_counter;
-		_z_reset_counter = local_pos.z_reset_counter;
-		_heading_reset_counter = local_pos.heading_reset_counter;
 	}
 
 	perf_end(_cycle_perf);
 }
 
 void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint,
-		const PositionControlStates &states, bool warn)
+		const PositionControlStates &states)
 {
+	// do not warn while we are disarmed, as we might not have valid setpoints yet
+	bool warn = _vehicle_control_mode.flag_armed && ((now - _last_warn) > 2_s);
+
+	if (warn) {
+		PX4_WARN("invalid setpoints");
+		_last_warn = now;
+	}
+
 	// Only react after a short delay
 	_failsafe_land_hysteresis.set_state_and_update(true, now);
 
@@ -578,8 +613,6 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
 				PX4_WARN("Failsafe: blind descend");
 			}
 		}
-
-		_in_failsafe = true;
 	}
 }
 

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -92,21 +92,22 @@ private:
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 
-	uORB::PublicationData<takeoff_status_s>              _takeoff_status_pub {ORB_ID(takeoff_status)};
-	uORB::Publication<vehicle_attitude_setpoint_s>	     _vehicle_attitude_setpoint_pub {ORB_ID(vehicle_attitude_setpoint)};
-	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub {ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
+	uORB::PublicationData<takeoff_status_s>              _takeoff_status_pub{ORB_ID(takeoff_status)};
+	uORB::Publication<vehicle_attitude_setpoint_s>	     _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
 
-	uORB::SubscriptionCallbackWorkItem _local_pos_sub {this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */
+	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */
 
-	uORB::SubscriptionInterval _parameter_update_sub {ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _hover_thrust_estimate_sub {ORB_ID(hover_thrust_estimate)};
-	uORB::Subscription _trajectory_setpoint_sub {ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _vehicle_constraints_sub {ORB_ID(vehicle_constraints)};
-	uORB::Subscription _vehicle_control_mode_sub {ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	hrt_abstime _time_stamp_last_loop{0};		/**< time stamp of last loop iteration */
+	hrt_abstime _time_position_control_enabled{0};
 
 	vehicle_local_position_setpoint_s _setpoint {};
 	vehicle_control_mode_s _vehicle_control_mode {};
@@ -183,8 +184,6 @@ private:
 
 	hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */
 
-	bool _in_failsafe{false};  /**< true if failsafe was entered within current cycle */
-
 	bool _hover_thrust_initialized{false};
 
 	/** Timeout in us for trajectory data to get considered invalid */
@@ -228,8 +227,7 @@ private:
 	 * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
 	 * to true, the failsafe will be initiated immediately.
 	 */
-	void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
-		      bool warn);
+	void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states);
 
 	/**
 	 * Reset setpoints to NAN

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -114,7 +114,7 @@ bool PositionControl::update(const float dt)
 		_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
 	}
 
-	// There has to be a valid output accleration and thrust setpoint otherwise something went wrong
+	// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
 	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
 	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));