refactor fw_pos_control_l1: replace BlockParam* with Param* classes

Also change naming convention: add _ prefix to class attributes

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -36,10 +36,12 @@
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
 FixedwingPositionControl::FixedwingPositionControl() :
-	SuperBlock(nullptr, "FW_POS"),
+	ModuleParams(nullptr),
 	_sub_airspeed(ORB_ID(airspeed)),
 	_sub_sensors(ORB_ID(sensor_bias)),
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+	_launchDetector(this),
+	_runway_takeoff(this)
 {
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
@@ -219,9 +221,7 @@ FixedwingPositionControl::parameters_update()
 	_fw_pos_ctrl_status.landing_flare_length = _landingslope.flare_length();
 	fw_pos_ctrl_status_publish();
 
-	/* Update Launch Detector Parameters */
+	updateParams();
-	_launchDetector.updateParams();
-	_runway_takeoff.updateParams();
 
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -54,8 +54,6 @@
 
 #include <cfloat>
 
-#include <controllib/block/BlockParam.hpp>
-#include <controllib/blocks.hpp>
 #include <drivers/drv_hrt.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <ecl/tecs/tecs.h>
@@ -64,6 +62,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_module.h>
+#include <px4_module_params.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
 #include <systemlib/perf_counter.h>
@@ -122,7 +121,7 @@ using uORB::Subscription;
 using namespace launchdetection;
 using namespace runwaytakeoff;
 
-class FixedwingPositionControl final : public control::SuperBlock, public ModuleBase<FixedwingPositionControl>
+class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams
 {
 public:
 	FixedwingPositionControl();

src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp

@@ -41,36 +41,34 @@
 
 #include "CatapultLaunchMethod.h"
 
+#include <px4_log.h>
+
 namespace launchdetection
 {
 
-CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) :
-	SuperBlock(parent, "CAT"),
+	ModuleParams(parent)
-	thresholdAccel(this, "A"),
-	thresholdTime(this, "T"),
-	motorDelay(this, "MDEL"),
-	pitchMaxPreThrottle(this, "PMAX")
 {
-	last_timestamp = hrt_absolute_time();
+	_last_timestamp = hrt_absolute_time();
 }
 
 void CatapultLaunchMethod::update(float accel_x)
 {
-	float dt = hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	float dt = hrt_elapsed_time(&_last_timestamp) * 1e-6f;
-	last_timestamp = hrt_absolute_time();
+	_last_timestamp = hrt_absolute_time();
 
 	switch (state) {
 	case LAUNCHDETECTION_RES_NONE:
 
 		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
-		if (accel_x > thresholdAccel.get()) {
+		if (accel_x > _thresholdAccel.get()) {
-			integrator += dt;
+			_integrator += dt;
 
-			if (integrator > thresholdTime.get()) {
+			if (_integrator > _thresholdTime.get()) {
-				if (motorDelay.get() > 0.0f) {
+				if (_motorDelay.get() > 0.0f) {
 					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
 					PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
-						 double(motorDelay.get()));
+						 double(_motorDelay.get()));
 
 				} else {
 					/* No motor delay set: go directly to enablemotors state */
@@ -88,10 +86,10 @@ void CatapultLaunchMethod::update(float accel_x)
 	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
 		/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
 		 * over to allow full throttle */
-		motorDelayCounter += dt;
+		_motorDelayCounter += dt;
 
-		if (motorDelayCounter > motorDelay.get()) {
+		if (_motorDelayCounter > _motorDelay.get()) {
-			PX4_WARN("Launch detected: state enablemotors");
+			PX4_INFO("Launch detected: state enablemotors");
 			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 		}
 
@@ -110,8 +108,8 @@ LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
 
 void CatapultLaunchMethod::reset()
 {
-	integrator = 0.0f;
+	_integrator = 0.0f;
-	motorDelayCounter = 0.0f;
+	_motorDelayCounter = 0.0f;
 	state = LAUNCHDETECTION_RES_NONE;
 }
 
@@ -122,7 +120,7 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault)
 		return pitchMaxDefault;
 
 	} else {
-		return pitchMaxPreThrottle.get();
+		return _pitchMaxPreThrottle.get();
 	}
 }
 

src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h

@@ -44,16 +44,15 @@
 #include "LaunchMethod.h"
 
 #include <drivers/drv_hrt.h>
-#include <controllib/blocks.hpp>
+#include <px4_module_params.h>
-#include <controllib/block/BlockParam.hpp>
 
 namespace launchdetection
 {
 
-class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
+class CatapultLaunchMethod : public LaunchMethod, public ModuleParams
 {
 public:
-	CatapultLaunchMethod(SuperBlock *parent);
+	CatapultLaunchMethod(ModuleParams *parent);
 	~CatapultLaunchMethod() override = default;
 
 	void update(float accel_x) override;
@@ -62,18 +61,20 @@ public:
 	float getPitchMax(float pitchMaxDefault) override;
 
 private:
-	hrt_abstime last_timestamp{0};
+	hrt_abstime _last_timestamp{0};
-	float integrator{0.0f};
+	float _integrator{0.0f};
-	float motorDelayCounter{0.0f};
+	float _motorDelayCounter{0.0f};
 
 	LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
 
-	control::BlockParamFloat thresholdAccel;
+	DEFINE_PARAMETERS(
-	control::BlockParamFloat thresholdTime;
+		(ParamFloat<px4::params::LAUN_CAT_A>) _thresholdAccel,
-	control::BlockParamFloat motorDelay;
+		(ParamFloat<px4::params::LAUN_CAT_T>) _thresholdTime,
-	control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
+		(ParamFloat<px4::params::LAUN_CAT_MDEL>) _motorDelay,
+		(ParamFloat<px4::params::LAUN_CAT_PMAX>) _pitchMaxPreThrottle /**< Upper pitch limit before throttle is turned on.
 						       Can be used to make sure that the AC does not climb
 						       too much while attached to a bungee */
+	)
 
 };
 

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp

@@ -40,40 +40,38 @@
 #include "CatapultLaunchMethod.h"
 #include "LaunchDetector.h"
 
+#include <px4_log.h>
+
 namespace launchdetection
 {
 
-LaunchDetector::LaunchDetector() :
+LaunchDetector::LaunchDetector(ModuleParams *parent) :
-	SuperBlock(nullptr, "LAUN"),
+	ModuleParams(parent)
-	launchdetection_on(this, "ALL_ON")
 {
 	/* init all detectors */
-	launchMethods[0] = new CatapultLaunchMethod(this);
+	_launchMethods[0] = new CatapultLaunchMethod(this);
-
-	/* update all parameters of all detectors */
-	updateParams();
 }
 
 LaunchDetector::~LaunchDetector()
 {
-	delete launchMethods[0];
+	delete _launchMethods[0];
 }
 
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	for (const auto launchMethod : launchMethods) {
+	for (const auto launchMethod : _launchMethods) {
 		launchMethod->reset();
 	}
 
 	/* Reset active launchdetector */
-	activeLaunchDetectionMethodIndex = -1;
+	_activeLaunchDetectionMethodIndex = -1;
 }
 
 void LaunchDetector::update(float accel_x)
 {
 	if (launchDetectionEnabled()) {
-		for (const auto launchMethod : launchMethods) {
+		for (const auto launchMethod : _launchMethods) {
 			launchMethod->update(accel_x);
 		}
 	}
@@ -82,18 +80,18 @@ void LaunchDetector::update(float accel_x)
 LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
 	if (launchDetectionEnabled()) {
-		if (activeLaunchDetectionMethodIndex < 0) {
+		if (_activeLaunchDetectionMethodIndex < 0) {
-			/* None of the active launchmethods has detected a launch, check all launchmethods */
+			/* None of the active _launchmethods has detected a launch, check all _launchmethods */
-			for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
+			for (unsigned i = 0; i < (sizeof(_launchMethods) / sizeof(_launchMethods[0])); i++) {
-				if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+				if (_launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
-					PX4_WARN("selecting launchmethod %d", i);
+					PX4_INFO("selecting launchmethod %d", i);
-					activeLaunchDetectionMethodIndex = i; // from now on only check this method
+					_activeLaunchDetectionMethodIndex = i; // from now on only check this method
-					return launchMethods[i]->getLaunchDetected();
+					return _launchMethods[i]->getLaunchDetected();
 				}
 			}
 
 		} else {
-			return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
+			return _launchMethods[_activeLaunchDetectionMethodIndex]->getLaunchDetected();
 		}
 	}
 
@@ -108,16 +106,16 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault)
 
 	/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
 	 * otherwise use the default limit */
-	if (activeLaunchDetectionMethodIndex < 0) {
+	if (_activeLaunchDetectionMethodIndex < 0) {
-		if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) {
+		if (sizeof(_launchMethods) / sizeof(LaunchMethod *) > 1) {
 			return pitchMaxDefault;
 
 		} else {
-			return launchMethods[0]->getPitchMax(pitchMaxDefault);
+			return _launchMethods[0]->getPitchMax(pitchMaxDefault);
 		}
 
 	} else {
-		return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
+		return _launchMethods[_activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
 	}
 }
 

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h

@@ -42,16 +42,15 @@
 #define LAUNCHDETECTOR_H
 
 #include "LaunchMethod.h"
-#include <controllib/blocks.hpp>
+#include <px4_module_params.h>
-#include <controllib/block/BlockParam.hpp>
 
 namespace launchdetection
 {
 
-class __EXPORT LaunchDetector : public control::SuperBlock
+class __EXPORT LaunchDetector : public ModuleParams
 {
 public:
-	LaunchDetector();
+	LaunchDetector(ModuleParams *parent);
 	~LaunchDetector() override;
 
 	LaunchDetector(const LaunchDetector &) = delete;
@@ -61,23 +60,25 @@ public:
 
 	void update(float accel_x);
 	LaunchDetectionResult getLaunchDetected();
-	bool launchDetectionEnabled() { return launchdetection_on.get(); }
+	bool launchDetectionEnabled() { return _launchdetection_on.get(); }
 
 	/* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
 	float getPitchMax(float pitchMaxDefault);
 
 private:
-	/* holds an index to the launchMethod in the array launchMethods
+	/* holds an index to the launchMethod in the array _launchMethods
 	 * which detected a Launch. If no launchMethod has detected a launch yet the
 	 * value is -1. Once one launchMethod has detected a launch only this
 	 * method is checked for further advancing in the state machine
 	 * (e.g. when to power up the motors)
 	 */
-	int activeLaunchDetectionMethodIndex{-1};
+	int _activeLaunchDetectionMethodIndex{-1};
 
-	LaunchMethod *launchMethods[1];
+	LaunchMethod *_launchMethods[1];
 
-	control::BlockParamBool launchdetection_on;
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::LAUN_ALL_ON>) _launchdetection_on
+	)
 };
 
 } // namespace launchdetection

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -43,8 +43,6 @@
 #include <math.h>
 
 #include "RunwayTakeoff.h"
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 
@@ -53,32 +51,15 @@ using matrix::Vector2f;
 namespace runwaytakeoff
 {
 
-RunwayTakeoff::RunwayTakeoff() :
+RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
-	SuperBlock(nullptr, "RWTO"),
+	ModuleParams(parent),
 	_state(),
 	_initialized(false),
 	_initialized_time(0),
 	_init_yaw(0),
 	_climbout(false),
-	_throttle_ramp_time(2 * 1e6),
+	_throttle_ramp_time(2 * 1e6)
-	_runway_takeoff_enabled(this, "TKOFF"),
-	_heading_mode(this, "HDG"),
-	_nav_alt(this, "NAV_ALT"),
-	_takeoff_throttle(this, "MAX_THR"),
-	_runway_pitch_sp(this, "PSP"),
-	_max_takeoff_pitch(this, "MAX_PITCH"),
-	_max_takeoff_roll(this, "MAX_ROLL"),
-	_min_airspeed_scaling(this, "AIRSPD_SCL"),
-	_airspeed_min(this, "FW_AIRSPD_MIN", false),
-	_climbout_diff(this, "FW_CLMBOUT_DIFF", false)
 {
-
-	updateParams();
-}
-
-RunwayTakeoff::~RunwayTakeoff()
-{
-
 }
 
 void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -46,8 +46,7 @@
 #include <math.h>
 
 #include <drivers/drv_hrt.h>
-#include <controllib/blocks.hpp>
+#include <px4_module_params.h>
-#include <controllib/block/BlockParam.hpp>
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 
@@ -62,11 +61,11 @@ enum RunwayTakeoffState {
 	FLY = 4 /**< fly towards takeoff waypoint */
 };
 
-class __EXPORT RunwayTakeoff : public control::SuperBlock
+class __EXPORT RunwayTakeoff : public ModuleParams
 {
 public:
-	RunwayTakeoff();
+	RunwayTakeoff(ModuleParams *parent);
-	~RunwayTakeoff();
+	~RunwayTakeoff() = default;
 
 	void init(float yaw, double current_lat, double current_lon);
 	void update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub);
@@ -91,7 +90,6 @@ public:
 
 	void reset();
 
-protected:
 private:
 	/** state variables **/
 	RunwayTakeoffState _state;
@@ -102,17 +100,18 @@ private:
 	unsigned _throttle_ramp_time;
 	matrix::Vector2f _start_wp;
 
-	/** parameters **/
+	DEFINE_PARAMETERS(
-	control::BlockParamBool _runway_takeoff_enabled;
+		(ParamBool<px4::params::RWTO_TKOFF>) _runway_takeoff_enabled,
-	control::BlockParamInt _heading_mode;
+		(ParamInt<px4::params::RWTO_HDG>) _heading_mode,
-	control::BlockParamFloat _nav_alt;
+		(ParamFloat<px4::params::RWTO_NAV_ALT>) _nav_alt,
-	control::BlockParamFloat _takeoff_throttle;
+		(ParamFloat<px4::params::RWTO_MAX_THR>) _takeoff_throttle,
-	control::BlockParamFloat _runway_pitch_sp;
+		(ParamFloat<px4::params::RWTO_PSP>) _runway_pitch_sp,
-	control::BlockParamFloat _max_takeoff_pitch;
+		(ParamFloat<px4::params::RWTO_MAX_PITCH>) _max_takeoff_pitch,
-	control::BlockParamFloat _max_takeoff_roll;
+		(ParamFloat<px4::params::RWTO_MAX_ROLL>) _max_takeoff_roll,
-	control::BlockParamFloat _min_airspeed_scaling;
+		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _min_airspeed_scaling,
-	control::BlockParamFloat _airspeed_min;
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _airspeed_min,
-	control::BlockParamFloat _climbout_diff;
+		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _climbout_diff
+	)
 
 };