flight tasks: refactor BlockParam -> Param and handle param updates

src/lib/FlightTasks/FlightTasks.cpp

@@ -45,31 +45,31 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return 0;
 
 	case FlightTaskIndex::Stabilized:
-		_current_task = new (&_task_union.stabilized) FlightTaskManualStabilized(this, "MANSTAB");
+		_current_task = new (&_task_union.stabilized) FlightTaskManualStabilized();
 		break;
 
 	case FlightTaskIndex::Altitude:
-		_current_task = new (&_task_union.altitude) FlightTaskManualAltitude(this, "MANALT");
+		_current_task = new (&_task_union.altitude) FlightTaskManualAltitude();
 		break;
 
 	case FlightTaskIndex::AltitudeSmooth:
-		_current_task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth(this, "MANALTSM");
+		_current_task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth();
 		break;
 
 	case FlightTaskIndex::Position:
-		_current_task = new (&_task_union.position) FlightTaskManualPosition(this, "MANPOS");
+		_current_task = new (&_task_union.position) FlightTaskManualPosition();
 		break;
 
 	case FlightTaskIndex::PositionSmooth:
-		_current_task = new (&_task_union.position_smooth) FlightTaskManualPositionSmooth(this, "MANPOSSM");
+		_current_task = new (&_task_union.position_smooth) FlightTaskManualPositionSmooth();
 		break;
 
 	case FlightTaskIndex::Orbit:
-		_current_task = new (&_task_union.orbit) FlightTaskOrbit(this, "ORB");
+		_current_task = new (&_task_union.orbit) FlightTaskOrbit();
 		break;
 
 	case FlightTaskIndex::Sport:
-		_current_task = new (&_task_union.sport) FlightTaskSport(this, "SPO");
+		_current_task = new (&_task_union.sport) FlightTaskSport();
 		break;
 
 	default:
@@ -111,6 +111,13 @@ int FlightTasks::switchTask(int new_task_index)
 	return -1;
 }
 
+void FlightTasks::handleParameterUpdate()
+{
+	if (_current_task) {
+		_current_task->handleParameterUpdate();
+	}
+}
+
 void FlightTasks::_updateCommand()
 {
 //	TODO: port flight task mavlink command to have support for this functionality

src/lib/FlightTasks/FlightTasks.hpp

@@ -67,12 +67,10 @@ enum class FlightTaskIndex : int {
 	Count // number of tasks
 };
 
-class FlightTasks : control::SuperBlock
+class FlightTasks
 {
 public:
-	FlightTasks() :
-		SuperBlock(nullptr, "TSK")
-	{}
+	FlightTasks() = default;
 
 	~FlightTasks()
 	{
@@ -119,6 +117,11 @@ public:
 	 */
 	bool isAnyTaskActive() const { return _current_task; }
 
+	/**
+	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
+	 */
+	void handleParameterUpdate();
+
 private:
 
 	/**

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -41,7 +41,7 @@
 
 #pragma once
 
-#include <controllib/blocks.hpp>
+#include <px4_module_params.h>
 #include <drivers/drv_hrt.h>
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_local_position.h>
@@ -51,11 +51,11 @@
 #include "../SubscriptionArray.hpp"
 
 
-class FlightTask : public control::Block
+class FlightTask : public ModuleParams
 {
 public:
-	FlightTask(control::SuperBlock *parent, const char *name) :
-		Block(parent, name)
+	FlightTask() :
+		ModuleParams(nullptr)
 	{ _resetSetpoints(); }
 
 	virtual ~FlightTask() = default;
@@ -98,6 +98,14 @@ public:
 
 	static const vehicle_local_position_setpoint_s empty_setpoint;
 
+	/**
+	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
+	 */
+	void handleParameterUpdate()
+	{
+		updateParams();
+	}
+
 protected:
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -40,14 +40,6 @@
 
 using namespace matrix;
 
-FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name) :
-	FlightTask(parent, name),
-	_stick_dz(parent, "MPC_HOLD_DZ", false),
-	_xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false),
-	_z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false)
-{
-}
-
 bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
 	if (!FlightTask::initializeSubscriptions(subscription_array)) {

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -46,7 +46,7 @@
 class FlightTaskManual : public FlightTask
 {
 public:
-	FlightTaskManual(control::SuperBlock *parent, const char *name);
+	FlightTaskManual() = default;
 
 	virtual ~FlightTaskManual() = default;
 
@@ -61,14 +61,19 @@ protected:
 	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::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
-	control::BlockParamFloat _stick_dz; /**< 0-deadzone around the center for the sticks */
 
+	float stickDeadzone() const { return _stick_dz.get(); }
 private:
 
+	bool _evaluateSticks(); /**< checks and sets stick inputs */
+
 	uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
 
-	control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction */
-	control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in z direction */
-
-	bool _evaluateSticks(); /**< checks and sets stick inputs */
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_HOLD_DZ>) _stick_dz, /**< 0-deadzone around the center for the sticks */
+					(ParamFloat<px4::params::MPC_XY_MAN_EXPO>)
+					_xy_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction */
+					(ParamFloat<px4::params::MPC_Z_MAN_EXPO>)
+					_z_vel_man_expo /**< ratio of exponential curve for stick input in z direction */
+				       )
 };

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -40,14 +40,6 @@
 
 using namespace matrix;
 
-FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent, const char *name) :
-	FlightTaskManualStabilized(parent, name),
-	_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
-	_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
-	_vel_hold_thr_z(parent, "MPC_HOLD_MAX_Z", false)
-
-{}
-
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	/* Reuse same scaling as for stabilized */

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp

@@ -44,17 +44,20 @@
 class FlightTaskManualAltitude : public FlightTaskManualStabilized
 {
 public:
-	FlightTaskManualAltitude(control::SuperBlock *parent, const char *name);
+	FlightTaskManualAltitude() = default;
 
 	virtual ~FlightTaskManualAltitude() = default;
 
 protected:
-	control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
-	control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */
-	control::BlockParamFloat _vel_hold_thr_z; /**< velocity threshold to switch back into vertical position hold */
-
 	void _updateAltitudeLock(); /**< checks for position lock */
 	void _updateSetpoints() override; /**< updates all setpoints */
 	void _scaleSticks() override; /**< scales sticks to velocity in z */
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _vel_max_down, /**< maximum speed allowed to go up */
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up, /**< maximum speed allowed to go down */
+					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>)
+					_vel_hold_thr_z /**< velocity threshold to switch back into vertical position hold */
+				       )
+
 };

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp

@@ -39,9 +39,8 @@
 
 using namespace matrix;
 
-FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth(control::SuperBlock *parent, const char *name) :
-	FlightTaskManualAltitude(parent, name),
 	_smoothing(_velocity(2), _sticks(2))
+FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
 {}
 
 void FlightTaskManualAltitudeSmooth::_updateSetpoints()

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.hpp

@@ -44,7 +44,7 @@
 class FlightTaskManualAltitudeSmooth : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualAltitudeSmooth(control::SuperBlock *parent, const char *name);
+	FlightTaskManualAltitudeSmooth();
 	virtual ~FlightTaskManualAltitudeSmooth() = default;
 
 protected:

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -41,13 +41,6 @@
 
 using namespace matrix;
 
-FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent, const char *name) :
-	FlightTaskManualAltitude(parent, name),
-	_vel_xy_manual_max(parent, "MPC_VEL_MANUAL", false),
-	_acc_xy_max(parent, "MPC_ACC_HOR_MAX", false),
-	_vel_hold_thr_xy(parent,  "MPC_HOLD_MAX_XY", false)
-{}
-
 void FlightTaskManualPosition::_scaleSticks()
 {
 	/* Use same scaling as for FlightTaskManualAltitude */

src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp

@@ -45,16 +45,22 @@
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualPosition(control::SuperBlock *parent, const char *name);
+	FlightTaskManualPosition() = default;
 
 	virtual ~FlightTaskManualPosition() = default;
 
 protected:
-	control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
-	control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */
-	control::BlockParamFloat _vel_hold_thr_xy; /**< velocity threshold to switch back into horizontal position hold */
-
 	void _updateXYlock(); /**< applies positon lock based on stick and velocity */
 	void _updateSetpoints() override;
 	void _scaleSticks() override;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_xy_manual_max, /**< maximum speed allowed horizontally */
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>)
+					_acc_xy_max, /**< maximum acceleration horizontally. Only used to compute lock time */
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>)
+					_vel_hold_thr_xy /**< velocity threshold to switch back into horizontal position hold */
+				       )
+private:
+
 };

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

@@ -39,8 +39,7 @@
 
 using namespace matrix;
 
-FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth(control::SuperBlock *parent, const char *name) :
-	FlightTaskManualPosition(parent, name),
+FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
 	_smoothingXY(matrix::Vector2f(&_velocity(0))),
 	_smoothingZ(_velocity(2), _sticks(2))
 {}

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp

@@ -45,13 +45,15 @@
 class FlightTaskManualPositionSmooth : public FlightTaskManualPosition
 {
 public:
-	FlightTaskManualPositionSmooth(control::SuperBlock *parent, const char *name);
+	FlightTaskManualPositionSmooth();
 
 	virtual ~FlightTaskManualPositionSmooth() = default;
 
 protected:
 
 	virtual void _updateSetpoints() override;
+
+private:
 	ManualSmoothingXY _smoothingXY; /**< smoothing for velocity setpoints in xy */
 	ManualSmoothingZ _smoothingZ; /**< smoothing for velocity in z */
 };

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -40,15 +40,6 @@
 
 using namespace matrix;
 
-FlightTaskManualStabilized::FlightTaskManualStabilized(control::SuperBlock *parent, const char *name) :
-	FlightTaskManual(parent, name),
-	_yaw_rate_scaling(parent, "MPC_MAN_Y_MAX", false),
-	_tilt_max_man(parent, "MPC_MAN_TILT_MAX", false),
-	_throttle_min(parent, "MPC_THR_MIN", false),
-	_throttle_max(parent, "MPC_THR_MAX", false),
-	_throttle_hover(parent, "MPC_THR_HOVER", false)
-{}
-
 bool FlightTaskManualStabilized::activate()
 {
 	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
@@ -68,7 +59,7 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 	/* Yaw-lock depends on stick input. If not locked,
 	 * yaw_sp is set to NAN.
 	 * TODO: add yawspeed to get threshold.*/
-	const bool stick_yaw_zero = fabsf(_sticks(3)) <= _stick_dz.get();
+	const bool stick_yaw_zero = fabsf(_sticks(3)) <= stickDeadzone();
 
 	if (stick_yaw_zero && !PX4_ISFINITE(_yaw_setpoint)) {
 		_yaw_setpoint = _yaw;

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp

@@ -45,7 +45,7 @@
 class FlightTaskManualStabilized : public FlightTaskManual
 {
 public:
-	FlightTaskManualStabilized(control::SuperBlock *parent, const char *name);
+	FlightTaskManualStabilized() = default;
 
 	virtual ~FlightTaskManualStabilized() = default;
 
@@ -60,15 +60,17 @@ protected:
 
 private:
 
-	float _throttle{}; /** mapped from stick z */
-
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void _updateThrustSetpoints(); /**< sets thrust setpoint */
 	float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */
 
-	control::BlockParamFloat _yaw_rate_scaling; /**< scaling factor from stick to yaw rate */
-	control::BlockParamFloat _tilt_max_man; /**< maximum tilt allowed for manual flight */
-	control::BlockParamFloat _throttle_min; /**< minimum throttle that always has to be satisfied in flight*/
-	control::BlockParamFloat _throttle_max; /**< maximum throttle that always has to be satisfied in flight*/
-	control::BlockParamFloat _throttle_hover; /**< throttle value at which vehicle is at hover equilibrium */
+	float _throttle{}; /** mapped from stick z */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
+					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _tilt_max_man, /**< maximum tilt allowed for manual flight */
+					(ParamFloat<px4::params::MPC_THR_MIN>) _throttle_min, /**< minimum throttle that always has to be satisfied in flight*/
+					(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/
+					(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover /**< throttle value at which vehicle is at hover equilibrium */
+				       )
 };

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -40,8 +40,7 @@
 
 using namespace matrix;
 
-FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name) :
-	FlightTaskManual(parent, name)
+FlightTaskOrbit::FlightTaskOrbit()
 {
 	_sticks_data_required = false;
 }

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -46,7 +46,7 @@
 class FlightTaskOrbit : public FlightTaskManual
 {
 public:
-	FlightTaskOrbit(control::SuperBlock *parent, const char *name);
+	FlightTaskOrbit();
 
 	virtual ~FlightTaskOrbit() = default;
 

src/lib/FlightTasks/tasks/FlightTaskSport.hpp

@@ -48,10 +48,7 @@ using namespace matrix;
 class FlightTaskSport : public FlightTaskManualPosition
 {
 public:
-	FlightTaskSport(control::SuperBlock *parent, const char *name) :
-		FlightTaskManualPosition(parent, name),
-		_vel_xy_max(parent, "MPC_XY_VEL_MAX", false)
-	{ }
+	FlightTaskSport() = default;
 
 	virtual ~FlightTaskSport() = default;
 
@@ -70,6 +67,9 @@ protected:
 	}
 
 private:
-	control::BlockParamFloat _vel_xy_max; /**< maximal allowed horizontal speed, in sport mode full stick input*/
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>)
+					_vel_xy_max /**< maximal allowed horizontal speed, in sport mode full stick input*/
+				       )
 
 };

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -562,6 +562,8 @@ MulticopterPositionControl::parameters_update(bool force)
 		ModuleParams::updateParams();
 		SuperBlock::updateParams();
 
+		_flight_tasks.handleParameterUpdate();
+
 		/* initialize vectors from params and enforce constraints */
 
 		_pos_p(0) = _xy_p.get();