FlightTasks: move stick handling into library

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

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskManual
 	FlightTaskManual.cpp
 )
 
-target_link_libraries(FlightTaskManual PUBLIC FlightTask)
+target_link_libraries(FlightTaskManual PUBLIC FlightTask FlightTaskUtility)
 target_include_directories(FlightTaskManual PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp

@@ -42,13 +42,15 @@
 using namespace matrix;
 using namespace time_literals;
 
+FlightTaskManual::FlightTaskManual() :
+	_sticks(this)
+{};
+
 bool FlightTaskManual::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
 
-	_sub_manual_control_setpoint.update();
+	const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear);
-
-	const bool sticks_available = _evaluateSticks();
 
 	if (_sticks_data_required) {
 		ret = ret && sticks_available;
@@ -56,61 +58,3 @@ bool FlightTaskManual::updateInitialize()
 
 	return ret;
 }
-
-bool FlightTaskManual::_evaluateSticks()
-{
-	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
-
-	/* Sticks are rescaled linearly and exponentially to [-1,1] */
-	if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
-
-		/* Linear scale  */
-		_sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
-		_sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
-		_sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
-		_sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
-
-		/* Exponential scale */
-		_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
-
-		// Only switch the landing gear up if the user switched from gear down to gear up.
-		// If the user had the switch in the gear up position and took off ignore it
-		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
-		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
-
-		if (_gear_switch_old != gear_switch) {
-			_applyGearSwitch(gear_switch);
-		}
-
-		_gear_switch_old = gear_switch;
-
-		// valid stick inputs are required
-		const bool valid_sticks =  PX4_ISFINITE(_sticks(0))
-					   && PX4_ISFINITE(_sticks(1))
-					   && PX4_ISFINITE(_sticks(2))
-					   && PX4_ISFINITE(_sticks(3));
-
-		return valid_sticks;
-
-	} else {
-		/* Timeout: set all sticks to zero */
-		_sticks.zero();
-		_sticks_expo.zero();
-		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
-		return false;
-	}
-}
-
-void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
-{
-	if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-		_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-	}
-
-	if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
-		_gear.landing_gear = landing_gear_s::GEAR_UP;
-	}
-}

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp

@@ -41,42 +41,16 @@
 #pragma once
 
 #include "FlightTask.hpp"
-#include <uORB/topics/manual_control_setpoint.h>
+#include "Sticks.hpp"
 
 class FlightTaskManual : public FlightTask
 {
 public:
-	FlightTaskManual() = default;
+	FlightTaskManual();
-
 	virtual ~FlightTaskManual() = default;
-
-	bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); };
 	bool updateInitialize() override;
 
 protected:
-
+	Sticks _sticks;
 	bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */
-	matrix::Vector<float, 4> _sticks; /**< unmodified manual stick inputs */
-	matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
-	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/
-
-	float stickDeadzone() const { return _param_mpc_hold_dz.get(); }
-
-private:
-
-	bool _evaluateSticks(); /**< checks and sets stick inputs */
-	void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
-
-	uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
-
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
-					(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
-					_param_mpc_xy_man_expo, /**< ratio of exponential curve for stick input in xy direction */
-					(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
-					_param_mpc_z_man_expo, /**< ratio of exponential curve for stick input in z direction */
-					(ParamFloat<px4::params::MPC_YAW_EXPO>)
-					_param_mpc_yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */
-					(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t /**< time at which commander considers RC lost */
-				       )
 };

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -88,12 +88,12 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
-	const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
+	const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
 	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
 
 	// Use sticks input with deadzone and exponential curve for vertical velocity
-	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
+	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
-	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
+	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
 
 float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -110,7 +110,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	// If not locked, altitude setpoint is set to NAN.
 
 	// Check if user wants to break
-	const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON;
+	const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
 
 	// Check if vehicle has stopped
 	const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
@@ -122,7 +122,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 		float spd_xy = Vector2f(_velocity).length();
 
 		// Use presence of horizontal stick inputs as a transition criteria
-		float stick_xy = Vector2f(&_sticks_expo(0)).length();
+		float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length();
 		bool stick_input = stick_xy > 0.001f;
 
 		if (_terrain_hold) {
@@ -340,7 +340,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
 	// setpoint along z-direction, which is computed in PositionControl.cpp.
 
-	Vector2f sp(&_sticks(0));
+	Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0));
 
 	_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
 	_man_input_filter.update(sp);
@@ -359,8 +359,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 bool FlightTaskManualAltitude::_checkTakeoff()
 {
-	// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
+	// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
-	return _sticks(2) < -0.3f;
+	return _sticks.getPosition()(2) < -0.3f;
 }
 
 bool FlightTaskManualAltitude::update()

src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp

@@ -40,7 +40,7 @@
 using namespace matrix;
 
 FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
-	_smoothing(this, _velocity(2), _sticks(2))
+	_smoothing(this, _velocity(2), _sticks.getPosition()(2))
 {}
 
 void FlightTaskManualAltitudeSmooth::_updateSetpoints()

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -82,7 +82,7 @@ void FlightTaskManualPosition::_scaleSticks()
 	FlightTaskManualAltitude::_scaleSticks();
 
 	/* Constrain length of stick inputs to 1 for xy*/
-	Vector2f stick_xy = _sticks_expo.slice<2, 1>(0, 0);
+	Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0);
 
 	const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
 

src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp

@@ -41,7 +41,7 @@ using namespace matrix;
 
 FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
 	_smoothingXY(this, Vector2f(_velocity)),
-	_smoothingZ(this, _velocity(2), _sticks(2))
+	_smoothingZ(this, _velocity(2), _sticks.getPosition()(2))
 {}
 
 void FlightTaskManualPositionSmooth::_updateSetpoints()

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -173,8 +173,8 @@ bool FlightTaskOrbit::update()
 	bool ret = FlightTaskManualAltitudeSmooth::update();
 
 	// stick input adjusts parameters within a fixed time frame
-	const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f);
+	const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
-	const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f);
+	const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
 
 	setRadius(r);
 	setVelocity(v);
@@ -257,7 +257,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position)
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
 		_yaw_setpoint = NAN;
-		_yawspeed_setpoint = _sticks_expo(3);
+		_yawspeed_setpoint = _sticks.getPositionExpo()(3);
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:

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

@@ -38,6 +38,7 @@ px4_add_library(FlightTaskUtility
 	VelocitySmoothing.cpp
 	ManualVelocitySmoothingXY.cpp
 	ManualVelocitySmoothingZ.cpp
+	Sticks.cpp
 )
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier)

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

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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 Sticks.cpp
+ */
+
+#include "Sticks.hpp"
+
+using namespace time_literals;
+
+Sticks::Sticks(ModuleParams *parent) :
+	ModuleParams(parent)
+{};
+
+bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear)
+{
+	_sub_manual_control_setpoint.update();
+
+	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
+
+	// Sticks are rescaled linearly and exponentially to [-1,1]
+	if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
+
+		// Linear scale
+		_positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1]
+		_positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1]
+		_positions(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
+		_positions(3) = _sub_manual_control_setpoint.get().r; // yaw [-1,1]
+
+		// Exponential scale
+		_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
+		_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
+
+		// Only switch the landing gear up if the user switched from gear down to gear up.
+		// If the user had the switch in the gear up position and took off ignore it
+		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
+
+		if (_gear_switch_old != gear_switch) {
+			applyGearSwitch(gear_switch, gear);
+		}
+
+		_gear_switch_old = gear_switch;
+
+		// valid stick inputs are required
+		const bool valid_sticks =  PX4_ISFINITE(_positions(0))
+					   && PX4_ISFINITE(_positions(1))
+					   && PX4_ISFINITE(_positions(2))
+					   && PX4_ISFINITE(_positions(3));
+
+		return valid_sticks;
+
+	} else {
+		// Timeout: set all sticks to zero
+		_positions.zero();
+		_positions_expo.zero();
+		gear.landing_gear = landing_gear_s::GEAR_KEEP;
+		return false;
+	}
+}
+
+void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear)
+{
+	if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		gear.landing_gear = landing_gear_s::GEAR_DOWN;
+	}
+
+	if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+}

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

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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 Sticks.hpp
+ *
+ * Library abstracting stick input from manual_control_setpoint
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/manual_control_setpoint.h>
+
+class Sticks : public ModuleParams
+{
+public:
+	Sticks(ModuleParams *parent);
+	~Sticks() = default;
+
+	bool evaluateSticks(hrt_abstime now, landing_gear_s &gear); ///< checks and sets stick inputs
+	void applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear); ///< Sets gears according to switch
+	const matrix::Vector<float, 4> &getPosition() { return _positions; };
+	const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
+private:
+	matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
+	matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
+	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state
+
+	uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
+		(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
+		(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
+		(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo,
+		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t
+	)
+};