FlightModeManager: Add task for position slow mode

msg/CMakeLists.txt

@@ -234,6 +234,7 @@ set(msg_files
 	VehicleTorqueSetpoint.msg
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
+	VelocityLimits.msg
 	VtolVehicleStatus.msg
 	WheelEncoders.msg
 	Wind.msg

msg/VelocityLimits.msg

@@ -0,0 +1,8 @@
+# Velocity and yaw rate limits for a multicopter position slow mode only
+
+uint64 timestamp # time since system start (microseconds)
+
+# absolute speeds, NAN means use default limit
+float32 horizontal_velocity # [m/s]
+float32 vertical_velocity # [m/s]
+float32 yaw_rate # [rad/s]

src/modules/flight_mode_manager/CMakeLists.txt

@@ -43,6 +43,7 @@ list(APPEND flight_tasks_all
 	Descend
 	Failsafe
 	ManualAcceleration
+	ManualAccelerationSlow
 	ManualAltitude
 	ManualAltitudeSmoothVel
 	ManualPosition

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -196,6 +196,13 @@ void FlightModeManager::start_flight_task()
 		}
 	}
 
+	// position slow mode
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) {
+		found_some_task = true;
+		FlightTaskError error = switchTask(FlightTaskIndex::ManualAccelerationSlow);
+		task_failure = error != FlightTaskError::NoError;
+	}
+
 	// Manual position control
 	if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) {
 		found_some_task = true;

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt

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

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 FlightTaskManualAccelerationSlow.cpp
+ */
+
+#include "FlightTaskManualAccelerationSlow.hpp"
+#include <px4_platform_common/events.h>
+
+using namespace time_literals;
+
+bool FlightTaskManualAccelerationSlow::update()
+{
+	// Used to apply a configured default slowdown if neither MAVLink nor remote knob commands limits
+	bool velocity_horizontal_limited = false;
+	bool velocity_vertical_limited = false;
+	bool yaw_rate_limited = false;
+
+	// Limits which can only slow down from the nominal configuration we initialize with here
+	// This is ensured by the executing classes
+	float velocity_horizontal = _param_mpc_vel_manual.get();
+	float velocity_up = _param_mpc_z_vel_max_up.get();
+	float velocity_down = _param_mpc_z_vel_max_dn.get();
+	float yaw_rate = math::radians(_param_mpc_man_y_max.get());
+
+	// MAVLink commanded limits
+	if (_velocity_limits_sub.update(&_velocity_limits)) {
+		_velocity_limits_received_before = true;
+	}
+
+	if (_velocity_limits_received_before) {
+		// message received once since mode was started
+		if (PX4_ISFINITE(_velocity_limits.horizontal_velocity)) {
+			velocity_horizontal = fmaxf(_velocity_limits.horizontal_velocity, _param_posslow_min_hvel.get());
+			velocity_horizontal_limited = true;
+		}
+
+		if (PX4_ISFINITE(_velocity_limits.vertical_velocity)) {
+			velocity_up = velocity_down = fmaxf(_velocity_limits.vertical_velocity, _param_posslow_min_vvel.get());
+			velocity_vertical_limited = true;
+		}
+
+		if (PX4_ISFINITE(_velocity_limits.yaw_rate)) {
+			yaw_rate = fmaxf(_velocity_limits.yaw_rate, math::radians(_param_posslow_min_yawr.get()));
+			yaw_rate_limited = true;
+		}
+	}
+
+	// Remote knob commanded limits
+	if (_param_posslow_map_hvel.get() != 0) {
+		const float min_horizontal_velocity_scale = _param_posslow_min_hvel.get() / fmaxf(velocity_horizontal, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_hvel.get());
+		const float aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_horizontal_velocity_scale, 1.f);
+		velocity_horizontal *= aux_based_scale;
+		velocity_horizontal_limited = true;
+	}
+
+	if (_param_posslow_map_vvel.get() != 0) {
+		const float min_up_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_up, FLT_EPSILON);
+		const float min_down_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_down, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_vvel.get());
+		const float up_aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_up_speed_scale, 1.f);
+		const float down_aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_down_speed_scale, 1.f);
+		velocity_up *= up_aux_based_scale;
+		velocity_down *= down_aux_based_scale;
+		velocity_vertical_limited = true;
+	}
+
+	if (_param_posslow_map_yawr.get() != 0) {
+		const float min_yaw_rate_scale = math::radians(_param_posslow_min_yawr.get()) / fmaxf(yaw_rate, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_yawr.get());
+		const float aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_yaw_rate_scale, 1.f);
+		yaw_rate *= aux_based_scale;
+		yaw_rate_limited = true;
+	}
+
+	// No input from remote and MAVLink -> use default slow mode limits
+	if (!velocity_horizontal_limited) {
+		velocity_horizontal = _param_posslow_def_hvel.get();
+	}
+
+	if (!velocity_vertical_limited) {
+		velocity_up = velocity_down = _param_posslow_def_vvel.get();
+	}
+
+	if (!yaw_rate_limited) {
+		yaw_rate = math::radians(_param_posslow_def_yawr.get());
+	}
+
+	// Interface to set resulting velocity limits
+	FlightTaskManualAcceleration::_stick_acceleration_xy.setVelocityConstraint(velocity_horizontal);
+	FlightTaskManualAltitude::_velocity_constraint_up = velocity_up;
+	FlightTaskManualAltitude::_velocity_constraint_down = velocity_down;
+	FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate);
+
+	return FlightTaskManualAcceleration::update();
+}
+
+float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value)
+{
+	const int sanitized_index = math::constrain(parameter_value - 1, 0, 5);
+	return _sticks.getAux()(sanitized_index);
+}

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 FlightTaskManualAccelerationSlow.hpp
+ *
+ * Flight task for manual position mode with knobs for slower velocity and acceleration.
+ *
+ */
+
+#pragma once
+
+#include "FlightTaskManualAcceleration.hpp"
+#include "StickAccelerationXY.hpp"
+
+#include <lib/weather_vane/WeatherVane.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/velocity_limits.h>
+#include <systemlib/mavlink_log.h>
+
+class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration
+{
+public:
+	FlightTaskManualAccelerationSlow() = default;
+	~FlightTaskManualAccelerationSlow() = default;
+	bool update() override;
+
+private:
+
+	/**
+	 * Get the input from a sanitized parameter aux index
+	 * @param parameter_value value of the parameter that specifies the AUX channel index to use
+	 * @return input from that AUX channel [-1,1]
+	 */
+	float getInputFromSanitizedAuxParameterIndex(int parameter_value);
+
+	bool _velocity_limits_received_before{false};
+
+	uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)};
+	velocity_limits_s _velocity_limits{};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration,
+					(ParamInt<px4::params::POSSLOW_MAP_HVEL>) _param_posslow_map_hvel,
+					(ParamInt<px4::params::POSSLOW_MAP_VVEL>) _param_posslow_map_vvel,
+					(ParamInt<px4::params::POSSLOW_MAP_YAWR>) _param_posslow_map_yawr,
+					(ParamFloat<px4::params::POSSLOW_MIN_HVEL>) _param_posslow_min_hvel,
+					(ParamFloat<px4::params::POSSLOW_MIN_VVEL>) _param_posslow_min_vvel,
+					(ParamFloat<px4::params::POSSLOW_MIN_YAWR>) _param_posslow_min_yawr,
+					(ParamFloat<px4::params::POSSLOW_DEF_HVEL>) _param_posslow_def_hvel,
+					(ParamFloat<px4::params::POSSLOW_DEF_VVEL>) _param_posslow_def_vvel,
+					(ParamFloat<px4::params::POSSLOW_DEF_YAWR>) _param_posslow_def_yawr,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
+				       )
+};

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c

@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Manual input mapped to scale horizontal velocity in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(POSSLOW_MAP_HVEL, 0);
+
+/**
+ * Manual input mapped to scale vertical velocity in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(POSSLOW_MAP_VVEL, 0);
+
+/**
+ * Manual input mapped to scale yaw rate in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(POSSLOW_MAP_YAWR, 0);
+
+/**
+ * Horizontal velocity lower limit
+ *
+ * The lowest input maps and is clamped to this velocity.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_MIN_HVEL, .3f);
+
+/**
+ * Vertical velocity lower limit
+ *
+ * The lowest input maps and is clamped to this velocity.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_MIN_VVEL, .3f);
+
+/**
+ * Yaw rate lower limit
+ *
+ * The lowest input maps and is clamped to this rate.
+ *
+ * @unit deg/s
+ * @min 1
+ * @increment 0.1
+ * @decimal 0
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_MIN_YAWR, 3.f);
+
+/**
+ * Default horizontal velocity limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_DEF_HVEL, 3.f);
+
+/**
+ * Default vertical velocity limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_DEF_VVEL, 1.f);
+
+/**
+ * Default yaw rate limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit deg/s
+ * @min 1
+ * @increment 0.1
+ * @decimal 0
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(POSSLOW_DEF_YAWR, 45.f);