From 10ba0801d268842647fdcf312fd120af067daf8e Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Mar 2019 11:12:42 +0100 Subject: [PATCH] Parameter update - Rename variables in modules/fw_pos_control using parameter_update.py script --- .../launchdetection/CatapultLaunchMethod.cpp | 12 ++++---- .../launchdetection/CatapultLaunchMethod.h | 8 +++--- .../launchdetection/LaunchDetector.h | 4 +-- .../runway_takeoff/RunwayTakeoff.cpp | 28 +++++++++---------- .../runway_takeoff/RunwayTakeoff.h | 24 ++++++++-------- 5 files changed, 38 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp index e277fd3c7e..3c63e30dcb 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp @@ -61,14 +61,14 @@ void CatapultLaunchMethod::update(float accel_x) 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 > _param_laun_cat_a.get()) { _integrator += dt; - if (_integrator > _thresholdTime.get()) { - if (_motorDelay.get() > 0.0f) { + if (_integrator > _param_laun_cat_t.get()) { + if (_param_laun_cat_mdel.get() > 0.0f) { state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", - double(_motorDelay.get())); + double(_param_laun_cat_mdel.get())); } else { /* No motor delay set: go directly to enablemotors state */ @@ -88,7 +88,7 @@ void CatapultLaunchMethod::update(float accel_x) * over to allow full throttle */ _motorDelayCounter += dt; - if (_motorDelayCounter > _motorDelay.get()) { + if (_motorDelayCounter > _param_laun_cat_mdel.get()) { PX4_INFO("Launch detected: state enablemotors"); state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } @@ -120,7 +120,7 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) return pitchMaxDefault; } else { - return _pitchMaxPreThrottle.get(); + return _param_laun_cat_pmax.get(); } } diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h index f356d85e23..9b13d8d45b 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h @@ -68,10 +68,10 @@ private: LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; DEFINE_PARAMETERS( - (ParamFloat) _thresholdAccel, - (ParamFloat) _thresholdTime, - (ParamFloat) _motorDelay, - (ParamFloat) _pitchMaxPreThrottle /**< Upper pitch limit before throttle is turned on. + (ParamFloat) _param_laun_cat_a, + (ParamFloat) _param_laun_cat_t, + (ParamFloat) _param_laun_cat_mdel, + (ParamFloat) _param_laun_cat_pmax /**< 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.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 30db9726c7..a01ef51644 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -60,7 +60,7 @@ public: void update(float accel_x); LaunchDetectionResult getLaunchDetected(); - bool launchDetectionEnabled() { return _launchdetection_on.get(); } + bool launchDetectionEnabled() { return _param_laun_all_on.get(); } /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ float getPitchMax(float pitchMaxDefault); @@ -77,7 +77,7 @@ private: LaunchMethod *_launchMethods[1]; DEFINE_PARAMETERS( - (ParamBool) _launchdetection_on + (ParamBool) _param_laun_all_on ) }; 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 722c3d5f3b..322f1bff5c 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -86,7 +86,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { + if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached"); } @@ -94,14 +94,14 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _nav_alt.get()) { + if (alt_agl > _param_rwto_nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; /* * If we started in heading hold mode, move the navigation start WP to the current location now. * The navigator will take this as starting point to navigate towards the takeoff WP. */ - if (_heading_mode.get() == 0) { + if (_param_rwto_hdg.get() == 0) { _start_wp(0) = (float)current_lat; _start_wp(1) = (float)current_lon; } @@ -112,7 +112,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, break; case RunwayTakeoffState::CLIMBOUT: - if (alt_agl > _climbout_diff.get()) { + if (alt_agl > _param_fw_clmbout_diff.get()) { _climbout = false; _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint"); @@ -143,7 +143,7 @@ bool RunwayTakeoff::controlYaw() float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_runway_pitch_sp.get()); + return math::radians(_param_rwto_psp.get()); } return tecsPitch; @@ -162,8 +162,8 @@ float RunwayTakeoff::getRoll(float navigatorRoll) // allow some roll during climbout else if (_state < RunwayTakeoffState::FLY) { return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll.get()), - math::radians(_max_takeoff_roll.get())); + math::radians(-_param_rwto_max_roll.get()), + math::radians(_param_rwto_max_roll.get())); } return navigatorRoll; @@ -177,7 +177,7 @@ float RunwayTakeoff::getRoll(float navigatorRoll) */ float RunwayTakeoff::getYaw(float navigatorYaw) { - if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { return _init_yaw; } else { @@ -196,14 +196,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * - _takeoff_throttle.get(); - return throttle < _takeoff_throttle.get() ? + _param_rwto_max_thr.get(); + return throttle < _param_rwto_max_thr.get() ? throttle : - _takeoff_throttle.get(); + _param_rwto_max_thr.get(); } case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _takeoff_throttle.get(); + return _param_rwto_max_thr.get(); default: return tecsThrottle; @@ -242,8 +242,8 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) float RunwayTakeoff::getMaxPitch(float max) { // use max pitch from parameter if set (> 0.1) - if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { - return _max_takeoff_pitch.get(); + if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) { + return _param_rwto_max_pitch.get(); } else { 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 90f06cbe4d..15c9e8b005 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -74,8 +74,8 @@ public: RunwayTakeoffState getState() { return _state; } bool isInitialized() { return _initialized; } - bool runwayTakeoffEnabled() { return _runway_takeoff_enabled.get(); } - float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); } + bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); } + float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); } float getInitYaw() { return _init_yaw; } bool controlYaw(); @@ -102,16 +102,16 @@ private: matrix::Vector2f _start_wp; 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 + (ParamBool) _param_rwto_tkoff, + (ParamInt) _param_rwto_hdg, + (ParamFloat) _param_rwto_nav_alt, + (ParamFloat) _param_rwto_max_thr, + (ParamFloat) _param_rwto_psp, + (ParamFloat) _param_rwto_max_pitch, + (ParamFloat) _param_rwto_max_roll, + (ParamFloat) _param_rwto_airspd_scl, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_clmbout_diff ) };