Browse Source

Parameter update - Rename variables in modules/fw_pos_control

using parameter_update.py script
sbg
bresch 6 years ago committed by Matthias Grob
parent
commit
10ba0801d2
  1. 12
      src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp
  2. 8
      src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h
  3. 4
      src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h
  4. 28
      src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
  5. 24
      src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

12
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp

@ -61,14 +61,14 @@ void CatapultLaunchMethod::update(float accel_x) @@ -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) @@ -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) @@ -120,7 +120,7 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault)
return pitchMaxDefault;
} else {
return _pitchMaxPreThrottle.get();
return _param_laun_cat_pmax.get();
}
}

8
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h

@ -68,10 +68,10 @@ private: @@ -68,10 +68,10 @@ private:
LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::LAUN_CAT_A>) _thresholdAccel,
(ParamFloat<px4::params::LAUN_CAT_T>) _thresholdTime,
(ParamFloat<px4::params::LAUN_CAT_MDEL>) _motorDelay,
(ParamFloat<px4::params::LAUN_CAT_PMAX>) _pitchMaxPreThrottle /**< Upper pitch limit before throttle is turned on.
(ParamFloat<px4::params::LAUN_CAT_A>) _param_laun_cat_a,
(ParamFloat<px4::params::LAUN_CAT_T>) _param_laun_cat_t,
(ParamFloat<px4::params::LAUN_CAT_MDEL>) _param_laun_cat_mdel,
(ParamFloat<px4::params::LAUN_CAT_PMAX>) _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 */
)

4
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h

@ -60,7 +60,7 @@ public: @@ -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: @@ -77,7 +77,7 @@ private:
LaunchMethod *_launchMethods[1];
DEFINE_PARAMETERS(
(ParamBool<px4::params::LAUN_ALL_ON>) _launchdetection_on
(ParamBool<px4::params::LAUN_ALL_ON>) _param_laun_all_on
)
};

28
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@ -86,7 +86,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, @@ -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, @@ -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, @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 {

24
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@ -74,8 +74,8 @@ public: @@ -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: @@ -102,16 +102,16 @@ private:
matrix::Vector2f _start_wp;
DEFINE_PARAMETERS(
(ParamBool<px4::params::RWTO_TKOFF>) _runway_takeoff_enabled,
(ParamInt<px4::params::RWTO_HDG>) _heading_mode,
(ParamFloat<px4::params::RWTO_NAV_ALT>) _nav_alt,
(ParamFloat<px4::params::RWTO_MAX_THR>) _takeoff_throttle,
(ParamFloat<px4::params::RWTO_PSP>) _runway_pitch_sp,
(ParamFloat<px4::params::RWTO_MAX_PITCH>) _max_takeoff_pitch,
(ParamFloat<px4::params::RWTO_MAX_ROLL>) _max_takeoff_roll,
(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _min_airspeed_scaling,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _airspeed_min,
(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _climbout_diff
(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
(ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
(ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
(ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
(ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
(ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
)
};

Loading…
Cancel
Save