[WIP] Intuitive manual position control by mapping stick input to acceleration

boards/beaglebone/blue/src/board_config.h

@@ -94,4 +94,3 @@ void rc_cleaning(void);
 #define rc_filter_butterworth_lowpass	rc_butterworth_lowpass
 
 #endif
-

platforms/posix/src/px4/common/tasks.cpp

@@ -449,4 +449,3 @@ int px4_prctl(int option, const char *arg2, px4_task_t pid)
 
 	return rv;
 }
-

src/lib/CMakeLists.txt

@@ -56,6 +56,7 @@ add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(rc)
+add_subdirectory(SlewRate)
 add_subdirectory(systemlib)
 add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)

src/lib/SlewRate/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(SlewRate
+	dummy.cpp
+)
+target_include_directories(SlewRate
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_link_libraries(SlewRate PUBLIC mathlib)
+
+px4_add_unit_gtest(SRC SlewRateTest.cpp LINKLIBS SlewRate)

src/lib/SlewRate/SlewRate.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SlewRate.hpp
+ *
+ * Library limit the rate of change of a value with a maximum slew rate.
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+template<typename Type>
+class SlewRate
+{
+public:
+	SlewRate() = default;
+	~SlewRate() = default;
+
+	/**
+	 * Set maximum rate of change for the value
+	 * @param slew_rate maximum rate of change
+	 */
+	void setSlewRate(const Type slew_rate) { _slew_rate = slew_rate; }
+
+	/**
+	 * Set value ignoring slew rate for initialization purpose
+	 * @param value new applied value
+	 */
+	void setForcedValue(const Type value) { _value = value; }
+
+	/**
+	 * Update slewrate
+	 * @param new_value desired new value
+	 * @param deltatime time in seconds since last update
+	 * @return actual value that complies with the slew rate
+	 */
+	Type update(const Type new_value, const float deltatime)
+	{
+		// Limit the rate of change of the value
+		const Type dvalue_desired = new_value - _value;
+		const Type dvalue_max = _slew_rate * deltatime;
+		const Type dvalue = constrain(dvalue_desired, -dvalue_max, dvalue_max);
+		_value += dvalue;
+		return _value;
+	}
+
+private:
+	Type _slew_rate; ///< maximum rate of change for the value
+	Type _value; ///< state to keep last value of the slew rate
+
+	float constrain(float value, float min, float max)
+	{
+		return math::constrain(value, min, max);
+	}
+
+	Vector3f constrain(const Vector3f &value, const Vector3f &min, const Vector3f &max)
+	{
+		Vector3f constrained;
+
+		for (int i = 0; i < 3; i++) {
+			constrained(i) = math::constrain(value(i), min(i), max(i));
+		}
+
+		return constrained;
+	}
+};

src/lib/SlewRate/SlewRateTest.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <SlewRate.hpp>
+
+TEST(SlewRateTest, SlewUpLimited)
+{
+	SlewRate<float> _slew_rate;
+	_slew_rate.setSlewRate(.1f);
+	_slew_rate.setForcedValue(-5.5f);
+
+	for (int i = 1; i <= 10; i++) {
+		EXPECT_FLOAT_EQ(_slew_rate.update(20.f, .2f), -5.5f + i * .02f);
+	}
+}
+
+TEST(SlewRateTest, SlewDownLimited)
+{
+	SlewRate<float> _slew_rate;
+	_slew_rate.setSlewRate(1.1f);
+	_slew_rate.setForcedValue(17.3f);
+
+	for (int i = 1; i <= 10; i++) {
+		EXPECT_FLOAT_EQ(_slew_rate.update(-50.f, .3f), 17.3f - i * .33f);
+	}
+}
+
+TEST(SlewRateTest, ReachValueSlewed)
+{
+	SlewRate<float> _slew_rate;
+	_slew_rate.setSlewRate(.2f);
+	_slew_rate.setForcedValue(8.f);
+
+	for (int i = 1; i <= 10; i++) {
+		EXPECT_FLOAT_EQ(_slew_rate.update(10.f, 1.f), 8.f + i * .2f);
+	}
+
+	for (int i = 1; i <= 10; i++) {
+		EXPECT_FLOAT_EQ(_slew_rate.update(10.f, 1.f), 10.f);
+	}
+}
+
+TEST(SlewRateTest, SlewVector3f)
+{
+	using namespace matrix;
+
+	SlewRate<Vector3f> _slew_rate;
+	_slew_rate.setSlewRate(Vector3f(1, 2, 3));
+	_slew_rate.setForcedValue(Vector3f(4, 5, 6));
+
+	EXPECT_EQ(_slew_rate.update(Vector3f(100, 100, 100), 1.f), Vector3f(5, 7, 9));
+	EXPECT_EQ(_slew_rate.update(Vector3f(5, 7, 9), 1.f), Vector3f(5, 7, 9));
+	EXPECT_EQ(_slew_rate.update(Vector3f(-100, 100, -100), 1.f), Vector3f(4, 9, 6));
+	EXPECT_EQ(_slew_rate.update(Vector3f(4.1f, 8.8f, 6.3f), 1.f), Vector3f(4.1f, 8.8f, 6.3f));
+}

src/lib/flight_tasks/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017 - 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
@@ -64,6 +64,7 @@ list(APPEND flight_tasks_all
 	Failsafe
 	Descend
 	Transition
+	ManualAcceleration
 	${flight_tasks_to_add}
 )
 

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -382,7 +382,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 	VelocitySmoothing::timeSynchronization(_trajectory, 3);
 
 	_jerk_setpoint = jerk_sp_smooth;
-	_acceleration_setpoint = accel_sp_smooth;
+	_acceleration_setpoint.setNaN(); // TODO: until smooth feed-forward is fixed
 	_velocity_setpoint = vel_sp_smooth;
 	_position_setpoint = pos_sp_smooth;
 }

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -57,7 +57,7 @@ bool FlightTaskAutoMapper::update()
 	// vehicle exits idle.
 
 	if (_type_previous == WaypointType::idle) {
-		_thrust_setpoint.setNaN();
+		_acceleration_setpoint.setNaN();
 	}
 
 	// during mission and reposition, raise the landing gears but only
@@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints()
 	// Send zero thrust setpoint
 	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
 	_velocity_setpoint.setNaN();
-	_thrust_setpoint.zero();
+	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f);
 }
 
 void FlightTaskAutoMapper::_prepareLandSetpoints()

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp

@@ -40,7 +40,7 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
+	_acceleration_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
 	// keep heading
 	_yaw_setpoint = _yaw;
 	return ret;
@@ -51,12 +51,12 @@ bool FlightTaskDescend::update()
 	if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
-		_thrust_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
 
 	} else {
-		// descend with constant thrust (crash landing)
+		// descend with constant acceleration (crash landing)
 		_velocity_setpoint(2) = NAN;
-		_thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f;
+		_acceleration_setpoint(2) = 0.3f;
 	}
 
 	return true;

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;
 	_velocity_setpoint.zero();
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
+	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, 0.3f);
 	_yaw_setpoint = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	return ret;
@@ -52,26 +52,25 @@ bool FlightTaskFailsafe::update()
 	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
 		// stay at current position setpoint
 		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
-		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.0f;
 
 	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
 		// don't move along xy
 		_position_setpoint(0) = _position_setpoint(1) = NAN;
-		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
 	}
 
 	if (PX4_ISFINITE(_position(2))) {
 		// stay at current altitude setpoint
 		_velocity_setpoint(2) = 0.0f;
-		_thrust_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
 
 	} else if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
 		_position_setpoint(2) = NAN;
-		_thrust_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
 	}
 
 	return true;
-
 }

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -51,8 +51,6 @@ class FlightTaskFailsafe : public FlightTask
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_THR_HOVER>)
-					_param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
 				       )
 };

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -96,10 +96,12 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 
 	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
 	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
-	_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
 	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
 	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
 
+	// deprecated, only kept for output logging
+	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
 	return vehicle_local_position_setpoint;
 }
 
@@ -109,7 +111,6 @@ void FlightTask::_resetSetpoints()
 	_velocity_setpoint.setNaN();
 	_acceleration_setpoint.setNaN();
 	_jerk_setpoint.setNaN();
-	_thrust_setpoint.setNaN();
 	_yaw_setpoint = _yawspeed_setpoint = NAN;
 }