FlightTasks: Move stick handling into library, remove FlightTaskManual

src/lib/flight_tasks/tasks/CMakeLists.txt

@@ -34,7 +34,6 @@
 # add the core Flight tasks that the additional flight tasks depend on
 add_subdirectory(FlightTask)
 add_subdirectory(Utility)
-add_subdirectory(Manual)
 add_subdirectory(Auto)
 add_subdirectory(AutoMapper)
 

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

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

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

@@ -42,17 +42,27 @@
 
 using namespace matrix;
 
+FlightTaskManualAltitude::FlightTaskManualAltitude() :
+	_sticks(this)
+{};
+
 bool FlightTaskManualAltitude::updateInitialize()
 {
-	bool ret = FlightTaskManual::updateInitialize();
+	bool ret = FlightTask::updateInitialize();
+
+	const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear);
+
+	if (_sticks_data_required) {
+		ret = ret && sticks_available;
+	}
 
 	// in addition to manual require valid position and velocity in D-direction and valid yaw
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
 bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
-	bool ret = FlightTaskManual::activate(last_setpoint);
+	bool ret = FlightTask::activate(last_setpoint);
 	_yaw_setpoint = NAN;
 	_yawspeed_setpoint = 0.f;
 	_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
@@ -88,12 +98,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;
-	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
+	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
+	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
 
 float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -110,7 +120,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 +132,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 +350,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,13 +369,13 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 bool FlightTaskManualAltitude::_checkTakeoff()
 {
-	// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
-	return _sticks(2) < -0.3f;
+	// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
+	return _sticks.getPosition()(2) < -0.3f;
 }
 
 bool FlightTaskManualAltitude::update()
 {
-	bool ret = FlightTaskManual::update();
+	bool ret = FlightTask::update();
 	_updateConstraintsFromEstimator();
 	_scaleSticks();
 	_updateSetpoints();

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

@@ -39,13 +39,14 @@
 
 #pragma once
 
-#include "FlightTaskManual.hpp"
+#include "FlightTask.hpp"
+#include "Sticks.hpp"
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
 
-class FlightTaskManualAltitude : public FlightTaskManual
+class FlightTaskManualAltitude : public FlightTask
 {
 public:
-	FlightTaskManualAltitude() = default;
+	FlightTaskManualAltitude();
 	virtual ~FlightTaskManualAltitude() = default;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool updateInitialize() override;
@@ -71,7 +72,10 @@ class FlightTaskManualAltitude : public FlightTaskManual
 	 */
 	void _updateAltitudeLock();
 
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
+	Sticks _sticks;
+	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,

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 v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f);
+	const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.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/Manual/FlightTaskManual.cpp → src/lib/flight_tasks/tasks/Utility/Sticks.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   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
@@ -30,87 +30,75 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 /**
- * @file FlightTaskManual.cpp
+ * @file Sticks.cpp
  */
 
-#include "FlightTaskManual.hpp"
-#include <mathlib/mathlib.h>
-#include <float.h>
-#include <drivers/drv_hrt.h>
+#include "Sticks.hpp"
 
-using namespace matrix;
 using namespace time_literals;
 
-bool FlightTaskManual::updateInitialize()
-{
-	bool ret = FlightTask::updateInitialize();
+Sticks::Sticks(ModuleParams *parent) :
+	ModuleParams(parent)
+{};
 
+bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear)
+{
 	_sub_manual_control_setpoint.update();
 
-	const bool sticks_available = _evaluateSticks();
-
-	if (_sticks_data_required) {
-		ret = ret && sticks_available;
-	}
-
-	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) {
+	// Sticks are rescaled linearly and exponentially to [-1,1]
+	if ((now - _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] */
+		// 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 */
-		_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());
+		// 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);
+			applyGearSwitch(gear_switch, gear);
 		}
 
 		_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));
+		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 */
-		_sticks.zero();
-		_sticks_expo.zero();
-		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
+		// Timeout: set all sticks to zero
+		_positions.zero();
+		_positions_expo.zero();
+		gear.landing_gear = landing_gear_s::GEAR_KEEP;
 		return false;
 	}
 }
 
-void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
+void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear)
 {
-	if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-		_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+	if (gear_switch == 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;
+	if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		gear.landing_gear = landing_gear_s::GEAR_UP;
 	}
 }