Refactor FlightTaskManualPositionSmoothVel

2026-10-20 14:53:34 +08:00 · 2019-09-19 17:31:49 +02:00
parent 080eedfd02
commit f5d7eb4d87
7 changed files with 510 additions and 215 deletions
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ManualVelocitySmoothingZ.hpp"
+
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos)
+{
+	_smoothing.reset(accel, vel, pos);
+
+	resetPositionLock();
+}
+void ManualVelocitySmoothingZ::resetPositionLock()
+{
+	_position_lock_active = false;
+	_position_setpoint_locked = NAN;
+}
+
+void ManualVelocitySmoothingZ::update(float dt, float velocity_target)
+{
+	// Update state
+	updateTrajectories(dt);
+
+	// Set max accel/vel/jerk
+	// Has to be done before _updateTrajDurations()
+	updateTrajConstraints(velocity_target);
+
+	// Lock or unlock position
+	// Has to be done before _updateTrajDurations()
+	checkPositionLock(velocity_target);
+
+	// Update durations
+	_smoothing.updateDurations(velocity_target);
+}
+
+void ManualVelocitySmoothingZ::updateTrajectories(float dt)
+{
+	_smoothing.updateTraj(dt);
+
+	_state.j = _smoothing.getCurrentJerk();
+	_state.a = _smoothing.getCurrentAcceleration();
+	_state.v = _smoothing.getCurrentVelocity();
+	_state.x = _smoothing.getCurrentPosition();
+}
+
+void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
+{
+	if (velocity_target < 0.f) { // up
+		_smoothing.setMaxAccel(_max_accel_up);
+		_smoothing.setMaxVel(_max_vel_up);
+
+	} else { // down
+		_smoothing.setMaxAccel(_max_accel_down);
+		_smoothing.setMaxVel(_max_vel_down);
+	}
+}
+
+void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
+{
+	/**
+	 * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
+	 * is continuous. We know that the output of the position loop (part of the velocity setpoint)
+	 * will suddenly become null
+	 * and only the feedforward (generated by this flight task) will remain.
+	 * This is why the previous input of the velocity controller
+	 * is used to set current velocity of the trajectory.
+	 */
+	if (fabsf(_state.v) < 0.1f &&
+	    fabsf(_state.a) < .2f &&
+	    fabsf(velocity_target) <= FLT_EPSILON) {
+		// Lock position
+		_position_lock_active = true;
+		_position_setpoint_locked = _state.x;
+
+	} else {
+		// Unlock position
+		if (_position_lock_active) {
+			 // Start the trajectory at the current velocity setpoint
+			_smoothing.setCurrentVelocity(_velocity_setpoint_feedback);
+			_position_setpoint_locked = NAN;
+		}
+
+		_position_lock_active = false;
+		_smoothing.setCurrentPosition(_position_estimate);
+	}
+}