diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac4d0d7396..8306b7b00d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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 */ - _launchDetector.updateParams(); - _runway_takeoff.updateParams(); + updateParams(); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index c44e179e94..8dbca7277f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -54,8 +54,6 @@ #include -#include -#include #include #include #include @@ -64,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -122,7 +121,7 @@ using uORB::Subscription; using namespace launchdetection; using namespace runwaytakeoff; -class FixedwingPositionControl final : public control::SuperBlock, public ModuleBase +class FixedwingPositionControl final : public ModuleBase, public ModuleParams { public: FixedwingPositionControl(); diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp index 59881440a7..e277fd3c7e 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp @@ -41,36 +41,34 @@ #include "CatapultLaunchMethod.h" +#include + namespace launchdetection { -CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : - SuperBlock(parent, "CAT"), - thresholdAccel(this, "A"), - thresholdTime(this, "T"), - motorDelay(this, "MDEL"), - pitchMaxPreThrottle(this, "PMAX") +CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) : + ModuleParams(parent) { - 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; - last_timestamp = hrt_absolute_time(); + float dt = hrt_elapsed_time(&_last_timestamp) * 1e-6f; + _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()) { - integrator += dt; + if (accel_x > _thresholdAccel.get()) { + _integrator += dt; - if (integrator > thresholdTime.get()) { - if (motorDelay.get() > 0.0f) { + if (_integrator > _thresholdTime.get()) { + 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()) { - PX4_WARN("Launch detected: state enablemotors"); + if (_motorDelayCounter > _motorDelay.get()) { + 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; - motorDelayCounter = 0.0f; + _integrator = 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(); } } diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h index 2a7c5612c1..f356d85e23 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h @@ -44,16 +44,15 @@ #include "LaunchMethod.h" #include -#include -#include +#include 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}; - float integrator{0.0f}; - float motorDelayCounter{0.0f}; + hrt_abstime _last_timestamp{0}; + float _integrator{0.0f}; + float _motorDelayCounter{0.0f}; LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; - control::BlockParamFloat thresholdAccel; - control::BlockParamFloat thresholdTime; - control::BlockParamFloat motorDelay; - control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on. + DEFINE_PARAMETERS( + (ParamFloat) _thresholdAccel, + (ParamFloat) _thresholdTime, + (ParamFloat) _motorDelay, + (ParamFloat) _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 */ + ) }; diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp index b6673e0e63..36f36f1ab0 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -40,40 +40,38 @@ #include "CatapultLaunchMethod.h" #include "LaunchDetector.h" +#include + namespace launchdetection { -LaunchDetector::LaunchDetector() : - SuperBlock(nullptr, "LAUN"), - launchdetection_on(this, "ALL_ON") +LaunchDetector::LaunchDetector(ModuleParams *parent) : + ModuleParams(parent) { /* init all detectors */ - launchMethods[0] = new CatapultLaunchMethod(this); - - /* update all parameters of all detectors */ - updateParams(); + _launchMethods[0] = new CatapultLaunchMethod(this); } 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) { - /* None of the active launchmethods has detected a launch, check all launchmethods */ - for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { - if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { - PX4_WARN("selecting launchmethod %d", i); - activeLaunchDetectionMethodIndex = i; // from now on only check this method - return launchMethods[i]->getLaunchDetected(); + if (_activeLaunchDetectionMethodIndex < 0) { + /* None of the active _launchmethods has detected a launch, check all _launchmethods */ + for (unsigned i = 0; i < (sizeof(_launchMethods) / sizeof(_launchMethods[0])); i++) { + if (_launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + PX4_INFO("selecting launchmethod %d", i); + _activeLaunchDetectionMethodIndex = i; // from now on only check this method + 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 (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) { + if (_activeLaunchDetectionMethodIndex < 0) { + 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); } } diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 98af290aae..30db9726c7 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -42,16 +42,15 @@ #define LAUNCHDETECTOR_H #include "LaunchMethod.h" -#include -#include +#include 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) _launchdetection_on + ) }; } // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index 678008246c..722c3d5f3b 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -43,8 +43,6 @@ #include #include "RunwayTakeoff.h" -#include -#include #include #include @@ -53,32 +51,15 @@ using matrix::Vector2f; namespace runwaytakeoff { -RunwayTakeoff::RunwayTakeoff() : - SuperBlock(nullptr, "RWTO"), +RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) : + ModuleParams(parent), _state(), _initialized(false), _initialized_time(0), _init_yaw(0), _climbout(false), - _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) + _throttle_ramp_time(2 * 1e6) { - - updateParams(); -} - -RunwayTakeoff::~RunwayTakeoff() -{ - } void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index cfe28d5537..70204d2f67 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -46,8 +46,7 @@ #include #include -#include -#include +#include #include #include @@ -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(); + RunwayTakeoff(ModuleParams *parent); + ~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 **/ - control::BlockParamBool _runway_takeoff_enabled; - control::BlockParamInt _heading_mode; - control::BlockParamFloat _nav_alt; - control::BlockParamFloat _takeoff_throttle; - control::BlockParamFloat _runway_pitch_sp; - control::BlockParamFloat _max_takeoff_pitch; - control::BlockParamFloat _max_takeoff_roll; - control::BlockParamFloat _min_airspeed_scaling; - control::BlockParamFloat _airspeed_min; - control::BlockParamFloat _climbout_diff; + DEFINE_PARAMETERS( + (ParamBool) _runway_takeoff_enabled, + (ParamInt) _heading_mode, + (ParamFloat) _nav_alt, + (ParamFloat) _takeoff_throttle, + (ParamFloat) _runway_pitch_sp, + (ParamFloat) _max_takeoff_pitch, + (ParamFloat) _max_takeoff_roll, + (ParamFloat) _min_airspeed_scaling, + (ParamFloat) _airspeed_min, + (ParamFloat) _climbout_diff + ) };