Add acceleration stick mapping for position mode

src/lib/CMakeLists.txt

@@ -59,6 +59,7 @@ add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(rc)
 add_subdirectory(sensor_calibration)
+add_subdirectory(slew_rate)
 add_subdirectory(systemlib)
 add_subdirectory(tecs)
 add_subdirectory(terrain_estimation)

src/lib/flight_tasks/CMakeLists.txt

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

src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2020 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(FlightTaskManualAcceleration
+	FlightTaskManualAcceleration.cpp
+)
+target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility)

src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 FlightTaskManualAcceleration.cpp
+ */
+
+#include "FlightTaskManualAcceleration.hpp"
+
+using namespace matrix;
+
+FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
+	_stick_acceleration_xy(this)
+{};
+
+bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
+
+	if (PX4_ISFINITE(last_setpoint.vx)) {
+		_velocity_setpoint.xy() = Vector2f(last_setpoint.vx, last_setpoint.vy);
+
+	} else {
+		_velocity_setpoint.xy() = Vector2f(_velocity);
+	}
+
+	if (PX4_ISFINITE(last_setpoint.acceleration[0])) {
+		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
+	}
+
+	return ret;
+}
+
+bool FlightTaskManualAcceleration::update()
+{
+	bool ret = FlightTaskManualAltitudeSmoothVel::update();
+
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
+				       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+	_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
+			_deltatime);
+	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
+
+	_constraints.want_takeoff = _checkTakeoff();
+	return ret;
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
+{
+	if (PX4_ISFINITE(_position_setpoint(0))) {
+		_position_setpoint(0) = _position(0);
+		_position_setpoint(1) = _position(1);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
+{
+	if (PX4_ISFINITE(_velocity_setpoint(0))) {
+		_velocity_setpoint(0) = _velocity(0);
+		_velocity_setpoint(1) = _velocity(1);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ()
+{
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		_position_setpoint(2) = _position(2);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ()
+{
+	if (PX4_ISFINITE(_velocity_setpoint(2))) {
+		_velocity_setpoint(2) = _velocity(2);
+	}
+}
+
+void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi)
+{
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		_yaw_setpoint += delta_psi;
+	}
+}

src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 FlightTaskManualPosition.hpp
+ *
+ * Flight task for manual position controlled mode.
+ *
+ */
+
+#pragma once
+
+#include "FlightTaskManualAltitudeSmoothVel.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
+
+class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
+{
+public:
+	FlightTaskManualAcceleration();
+	virtual ~FlightTaskManualAcceleration() = default;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool update() override;
+
+private:
+	StickAccelerationXY _stick_acceleration_xy;
+	StickYaw _stick_yaw;
+
+	void _ekfResetHandlerPositionXY() override;
+	void _ekfResetHandlerVelocityXY() override;
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+	void _ekfResetHandlerHeading(float delta_psi) override;
+};

src/lib/flight_tasks/tasks/Utility/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018 - 2020 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
@@ -39,9 +39,11 @@ px4_add_library(FlightTaskUtility
 	ManualVelocitySmoothingXY.cpp
 	ManualVelocitySmoothingZ.cpp
 	Sticks.cpp
+	StickAccelerationXY.cpp
+	StickYaw.cpp
 )
 
-target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier)
+target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate)
 target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility)

src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 spec{fic 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 StickAccelerationXY.cpp
+ */
+
+#include "StickAccelerationXY.hpp"
+
+#include <ecl/geo/geo.h>
+#include "Sticks.hpp"
+
+using namespace matrix;
+
+StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+	_brake_boost_filter.reset(1.f);
+	resetPosition();
+}
+
+void StickAccelerationXY::resetPosition()
+{
+	_position.setNaN();
+}
+
+void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
+{
+	_velocity = velocity;
+}
+
+void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
+{
+	_acceleration_slew_rate_x.setForcedValue(acceleration(0));
+	_acceleration_slew_rate_x.setForcedValue(acceleration(1));
+}
+
+void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
+		const float dt)
+{
+	// maximum commanded acceleration and velocity
+	Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
+	Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get());
+
+	acceleration_scale *= 2.f; // because of drag the average aceleration is half
+
+	// Map stick input to acceleration
+	Sticks::limitStickUnitLengthXY(stick_xy);
+	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
+	_acceleration = stick_xy.emult(acceleration_scale);
+	applyFeasibilityLimit(_acceleration, dt);
+
+	// Add drag to limit speed and brake again
+	_acceleration -= calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity);
+
+	applyTiltLimit(_acceleration);
+
+	// Generate velocity setpoint by forward integrating commanded acceleration
+	_velocity += Vector2f(_acceleration) * dt;
+
+	lockPosition(_velocity, pos, dt, _position);
+}
+
+void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
+{
+	pos_sp.xy() = _position;
+	vel_sp.xy() = _velocity;
+	acc_sp.xy() = _acceleration;
+}
+
+void StickAccelerationXY::applyFeasibilityLimit(Vector2f &acceleration, const float dt)
+{
+	// Apply jerk limit - acceleration slew rate
+	_acceleration_slew_rate_x.setSlewRate(_param_mpc_jerk_max.get());
+	_acceleration_slew_rate_y.setSlewRate(_param_mpc_jerk_max.get());
+	acceleration(0) = _acceleration_slew_rate_x.update(acceleration(0), dt);
+	acceleration(1) = _acceleration_slew_rate_y.update(acceleration(1), dt);
+}
+
+Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const float dt, const Vector2f &stick_xy,
+		const Vector2f &vel_sp)
+{
+	_brake_boost_filter.setParameters(dt, .8f);
+
+	if (stick_xy.norm_squared() < FLT_EPSILON) {
+		_brake_boost_filter.update(2.f);
+
+	} else {
+		_brake_boost_filter.update(1.f);
+	}
+
+	drag_coefficient *= _brake_boost_filter.getState();
+
+	return drag_coefficient.emult(vel_sp);
+}
+
+void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
+{
+	// Check if acceleration would exceed the tilt limit
+	const float acc = acceleration.length();
+	const float acc_tilt_max = tanf(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()) * CONSTANTS_ONE_G;
+
+	if (acc > acc_tilt_max) {
+		acceleration *= acc_tilt_max / acc;
+	}
+}
+
+void StickAccelerationXY::lockPosition(const Vector2f &vel_sp, const Vector3f &pos, const float dt, Vector2f &pos_sp)
+{
+	if (vel_sp.norm_squared() < FLT_EPSILON) {
+		if (!PX4_ISFINITE(pos_sp(0))) {
+			pos_sp = Vector2f(pos);
+		}
+
+		pos_sp += vel_sp * dt;
+
+	} else {
+		pos_sp.setNaN();
+
+	}
+}

src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 StickAccelerationXY.hpp
+ * @brief Generate horizontal position, velocity and acceleration from stick input
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <float.h> // TODO add this include to AlphaFilter since it's used there
+#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <matrix/math.hpp>
+
+#include "SlewRate.hpp"
+
+class StickAccelerationXY : public ModuleParams
+{
+public:
+	StickAccelerationXY(ModuleParams *parent);
+	~StickAccelerationXY() = default;
+
+	void resetPosition();
+	void resetVelocity(const matrix::Vector2f &velocity);
+	void resetAcceleration(const matrix::Vector2f &acceleration);
+	void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
+			       const float dt);
+	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
+
+private:
+	void applyFeasibilityLimit(matrix::Vector2f &acceleration, const float dt);
+	matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
+				       const matrix::Vector2f &vel_sp);
+	void applyTiltLimit(matrix::Vector2f &acceleration);
+	void lockPosition(const matrix::Vector2f &vel_sp, const matrix::Vector3f &pos, const float dt,
+			  matrix::Vector2f &pos_sp);
+
+	SlewRate<float> _acceleration_slew_rate_x;
+	SlewRate<float> _acceleration_slew_rate_y;
+	AlphaFilter<float> _brake_boost_filter;
+
+	matrix::Vector2f _position;
+	matrix::Vector2f _velocity;
+	matrix::Vector2f _acceleration;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
+	)
+};

src/lib/flight_tasks/tasks/Utility/StickYaw.cpp

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 StickYaw.cpp
+ */
+
+#include "StickYaw.hpp"
+
+#include <px4_platform_common/defines.h>
+
+void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
+				   const float yaw, const float deltatime)
+{
+	_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
+	yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
+	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint);
+}
+
+float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint)
+{
+	// Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN.
+	if (fabsf(yawspeed_setpoint) > FLT_EPSILON) {
+		// no fixed heading when rotating around yaw by stick
+		return NAN;
+
+	} else {
+		// break down and hold the current heading when no more rotation commanded
+		if (!PX4_ISFINITE(yaw_setpoint)) {
+			return yaw;
+
+		} else {
+			return yaw_setpoint;
+		}
+	}
+}

src/lib/flight_tasks/tasks/Utility/StickYaw.hpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 StickYaw.hpp
+ * @brief Generate yaw and angular yawspeed setpoints from stick input
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "SlewRate.hpp"
+
+class StickYaw
+{
+public:
+	StickYaw() = default;
+	~StickYaw() = default;
+
+	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw,
+				 const float deltatime);
+
+private:
+	SlewRate<float> _yawspeed_slew_rate;
+
+	/**
+	 * Lock yaw when not currently turning
+	 * When applying a yawspeed the vehicle is turning, when the speed is
+	 * set to zero the vehicle needs to slow down and then lock at the yaw
+	 * it stops at to not drift over time.
+	 * @param yawspeed current yaw rotational rate state
+	 * @param yaw current yaw rotational rate state
+	 * @param yawspeed_setpoint rotation rate at which to turn around yaw axis
+	 * @param yaw current yaw setpoint which then will be overwritten by the return value
+	 * @return yaw setpoint to execute to have a yaw lock at the correct moment in time
+	 */
+	static float updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint);
+};

src/lib/flight_tasks/tasks/Utility/Sticks.cpp

@@ -38,6 +38,7 @@
 #include "Sticks.hpp"
 
 using namespace time_literals;
+using namespace matrix;
 
 Sticks::Sticks(ModuleParams *parent) :
 	ModuleParams(parent)
@@ -108,3 +109,19 @@ void Sticks::setGearAccordingToSwitch(landing_gear_s &gear)
 		_gear_switch_old = gear_switch;
 	}
 }
+
+void Sticks::limitStickUnitLengthXY(Vector2f &v)
+{
+	const float vl = v.length();
+
+	if (vl > 1.0f) {
+		v /= vl;
+	}
+}
+
+void Sticks::rotateIntoHeadingFrameXY(Vector2f &v, const float yaw, const float yaw_setpoint)
+{
+	Vector3f v3(v(0), v(1), 0.f);
+	const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw;
+	v = Vector2f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v3);
+}

src/lib/flight_tasks/tasks/Utility/Sticks.hpp

@@ -59,6 +59,21 @@ public:
 	const matrix::Vector<float, 4> &getPosition() { return _positions; };
 	const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
 
+
+	/**
+	 * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
+	 * @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
+	 */
+	static void limitStickUnitLengthXY(matrix::Vector2f &v);
+
+	/**
+	 * Rotate horizontal component of joystick input into the vehicle body orientation
+	 * @param v Vector containing x, y, z axis of the joystick input.
+	 * @param yaw Current vehicle yaw heading
+	 * @param yaw_setpoint Current yaw setpoint if it's locked else NAN
+	 */
+	static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
+
 private:
 	bool _input_available = false;
 	matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs

src/lib/slew_rate/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2020 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/slew_rate/SlewRate.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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>
+
+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 = math::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
+};

src/lib/slew_rate/SlewRateTest.cpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 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);
+	}
+}

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -597,6 +597,10 @@ void MulticopterPositionControl::start_flight_task()
 			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
 			break;
 
+		case 4:
+			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualAcceleration);
+			break;
+
 		default:
 			error =  _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
 			break;

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -463,7 +463,10 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
 /**
  * Maximum horizontal acceleration for auto mode and for manual mode
  *
- * Maximum deceleration for MPC_POS_MODE 1. Maximum acceleration and deceleration for MPC_POS_MODE 3.
+ * MPC_POS_MODE
+ * 1 just deceleration
+ * 3 acceleration and deceleration
+ * 4 just acceleration
  *
  * @unit m/s^2
  * @min 2.0
@@ -536,7 +539,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f);
  *
  * Setting this to the maximum value essentially disables the limit.
  *
- * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 1 or 3.
+ * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 1, 3 or 4.
  *
  * @unit m/s^3
  * @min 0.5
@@ -727,13 +730,15 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f);
  * 1 Smooth position control with maximum acceleration and jerk limits based on slew-rates.
  * 3 Smooth position control with maximum acceleration and jerk limits based on
  *   jerk optimized trajectory generator (different algorithm than 1).
+ * 4 Smooth position control where sticks map to acceleration and there's a virtual brake drag
  *
  * @value 0 Simple position control
  * @value 1 Smooth position control
  * @value 3 Smooth position control (Jerk optimized)
+ * @value 4 Acceleration based input
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_INT32(MPC_POS_MODE, 3);
+PARAM_DEFINE_INT32(MPC_POS_MODE, 4);
 
 /**
  * Enforced delay between arming and takeoff