mc_pos_control: use local position timestamp for dt

src/modules/mc_pos_control/Takeoff/Takeoff.hpp

@@ -59,7 +59,7 @@ public:
 	~Takeoff() = default;
 
 	// initialize parameters
-	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
+	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); }
 	void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
 
 	/**

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -234,12 +234,6 @@ private:
 	 */
 	void limit_altitude(vehicle_local_position_setpoint_s &setpoint);
 
-	/**
-	 * Prints a warning message at a lowered rate.
-	 * @param str the message that has to be printed.
-	 */
-	void warn_rate_limited(const char *str);
-
 	/**
 	 * Adjust the setpoint during landing.
 	 * Thrust is adjusted to support the land-detector during detection.
@@ -260,8 +254,8 @@ 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(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
-		      bool warn);
+	void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
+		      const bool force, bool warn);
 
 	/**
 	 * Reset setpoints to NAN
@@ -324,17 +318,6 @@ MulticopterPositionControl::init()
 	return true;
 }
 
-void
-MulticopterPositionControl::warn_rate_limited(const char *string)
-{
-	hrt_abstime now = hrt_absolute_time();
-
-	if (now - _last_warn > 200_ms) {
-		PX4_WARN("%s", string);
-		_last_warn = now;
-	}
-}
-
 int
 MulticopterPositionControl::parameters_update(bool force)
 {
@@ -542,11 +525,13 @@ MulticopterPositionControl::Run()
 		poll_subscriptions();
 		parameters_update(false);
 
-		// set _dt in controllib Block - the time difference since the last loop iteration in seconds
-		const hrt_abstime time_stamp_now = hrt_absolute_time();
-		setDt((time_stamp_now - _time_stamp_last_loop) / 1e6f);
+		const hrt_abstime time_stamp_now = _local_pos.timestamp;
+		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
 		_time_stamp_last_loop = time_stamp_now;
 
+		// set _dt in controllib Block - the time difference since the last loop iteration in seconds
+		setDt(dt);
+
 		const bool was_in_failsafe = _in_failsafe;
 		_in_failsafe = false;
 
@@ -587,7 +572,7 @@ MulticopterPositionControl::Run()
 			if (!_flight_tasks.update()) {
 				// FAILSAFE
 				// Task was not able to update correctly. Do Failsafe.
-				failsafe(setpoint, _states, false, !was_in_failsafe);
+				failsafe(time_stamp_now, setpoint, _states, false, !was_in_failsafe);
 
 			} else {
 				setpoint = _flight_tasks.getPositionSetpoint();
@@ -624,7 +609,7 @@ MulticopterPositionControl::Run()
 			// handle smooth takeoff
 			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
 						    constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
-			constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
+			constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up);
 
 			const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
 			const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
@@ -660,12 +645,16 @@ MulticopterPositionControl::Run()
 			_control.setConstraints(constraints);
 			_control.setInputSetpoint(setpoint);
 
-			if (!_control.update(_dt)) {
-				warn_rate_limited("PositionControl: invalid setpoints");
-				failsafe(setpoint, _states, true, !was_in_failsafe);
+			if (!_control.update(dt)) {
+				if ((time_stamp_now - _last_warn) > 1_s) {
+					PX4_WARN("invalid setpoints");
+					_last_warn = time_stamp_now;
+				}
+
+				failsafe(time_stamp_now, setpoint, _states, true, !was_in_failsafe);
 				_control.setInputSetpoint(setpoint);
 				constraints = FlightTask::empty_constraints;
-				_control.update(_dt);
+				_control.update(dt);
 			}
 
 			// Fill local position, velocity and thrust setpoint.
@@ -931,15 +920,15 @@ MulticopterPositionControl::start_flight_task()
 }
 
 void
-MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
-				     const bool force, bool warn)
+MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint,
+				     const PositionControlStates &states, const bool force, bool warn)
 {
 	// do not warn while we are disarmed, as we might not have valid setpoints yet
 	if (!_control_mode.flag_armed) {
 		warn = false;
 	}
 
-	_failsafe_land_hysteresis.set_state_and_update(true, hrt_absolute_time());
+	_failsafe_land_hysteresis.set_state_and_update(true, now);
 
 	if (!_failsafe_land_hysteresis.get_state() && !force) {
 		// just keep current setpoint and don't do anything.