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)
case LAUNCHDETECTION_RES_NONE: case LAUNCHDETECTION_RES_NONE:
/* Detect a acceleration that is longer and stronger as the minimum given by the params */ /* 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; _integrator += dt;
if (_integrator > _thresholdTime.get()) { if (_integrator > _param_laun_cat_t.get()) {
if (_motorDelay.get() > 0.0f) { if (_param_laun_cat_mdel.get() > 0.0f) {
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
double(_motorDelay.get())); double(_param_laun_cat_mdel.get()));
} else { } else {
/* No motor delay set: go directly to enablemotors state */ /* No motor delay set: go directly to enablemotors state */
@ -88,7 +88,7 @@ void CatapultLaunchMethod::update(float accel_x)
* over to allow full throttle */ * over to allow full throttle */
_motorDelayCounter += dt; _motorDelayCounter += dt;
if (_motorDelayCounter > _motorDelay.get()) { if (_motorDelayCounter > _param_laun_cat_mdel.get()) {
PX4_INFO("Launch detected: state enablemotors"); PX4_INFO("Launch detected: state enablemotors");
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
} }
@ -120,7 +120,7 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault)
return pitchMaxDefault; return pitchMaxDefault;
} else { } 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:
LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::LAUN_CAT_A>) _thresholdAccel, (ParamFloat<px4::params::LAUN_CAT_A>) _param_laun_cat_a,
(ParamFloat<px4::params::LAUN_CAT_T>) _thresholdTime, (ParamFloat<px4::params::LAUN_CAT_T>) _param_laun_cat_t,
(ParamFloat<px4::params::LAUN_CAT_MDEL>) _motorDelay, (ParamFloat<px4::params::LAUN_CAT_MDEL>) _param_laun_cat_mdel,
(ParamFloat<px4::params::LAUN_CAT_PMAX>) _pitchMaxPreThrottle /**< Upper pitch limit before throttle is turned on. (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 Can be used to make sure that the AC does not climb
too much while attached to a bungee */ too much while attached to a bungee */
) )

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

@ -60,7 +60,7 @@ public:
void update(float accel_x); void update(float accel_x);
LaunchDetectionResult getLaunchDetected(); 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 */ /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
float getPitchMax(float pitchMaxDefault); float getPitchMax(float pitchMaxDefault);
@ -77,7 +77,7 @@ private:
LaunchMethod *_launchMethods[1]; LaunchMethod *_launchMethods[1];
DEFINE_PARAMETERS( 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,
break; break;
case RunwayTakeoffState::CLAMPED_TO_RUNWAY: 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; _state = RunwayTakeoffState::TAKEOFF;
mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached"); mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached");
} }
@ -94,14 +94,14 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
break; break;
case RunwayTakeoffState::TAKEOFF: case RunwayTakeoffState::TAKEOFF:
if (alt_agl > _nav_alt.get()) { if (alt_agl > _param_rwto_nav_alt.get()) {
_state = RunwayTakeoffState::CLIMBOUT; _state = RunwayTakeoffState::CLIMBOUT;
/* /*
* If we started in heading hold mode, move the navigation start WP to the current location now. * 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. * 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(0) = (float)current_lat;
_start_wp(1) = (float)current_lon; _start_wp(1) = (float)current_lon;
} }
@ -112,7 +112,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
break; break;
case RunwayTakeoffState::CLIMBOUT: case RunwayTakeoffState::CLIMBOUT:
if (alt_agl > _climbout_diff.get()) { if (alt_agl > _param_fw_clmbout_diff.get()) {
_climbout = false; _climbout = false;
_state = RunwayTakeoffState::FLY; _state = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint"); mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint");
@ -143,7 +143,7 @@ bool RunwayTakeoff::controlYaw()
float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getPitch(float tecsPitch)
{ {
if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
return math::radians(_runway_pitch_sp.get()); return math::radians(_param_rwto_psp.get());
} }
return tecsPitch; return tecsPitch;
@ -162,8 +162,8 @@ float RunwayTakeoff::getRoll(float navigatorRoll)
// allow some roll during climbout // allow some roll during climbout
else if (_state < RunwayTakeoffState::FLY) { else if (_state < RunwayTakeoffState::FLY) {
return math::constrain(navigatorRoll, return math::constrain(navigatorRoll,
math::radians(-_max_takeoff_roll.get()), math::radians(-_param_rwto_max_roll.get()),
math::radians(_max_takeoff_roll.get())); math::radians(_param_rwto_max_roll.get()));
} }
return navigatorRoll; return navigatorRoll;
@ -177,7 +177,7 @@ float RunwayTakeoff::getRoll(float navigatorRoll)
*/ */
float RunwayTakeoff::getYaw(float navigatorYaw) 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; return _init_yaw;
} else { } else {
@ -196,14 +196,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle)
switch (_state) { switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP: { case RunwayTakeoffState::THROTTLE_RAMP: {
float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time *
_takeoff_throttle.get(); _param_rwto_max_thr.get();
return throttle < _takeoff_throttle.get() ? return throttle < _param_rwto_max_thr.get() ?
throttle : throttle :
_takeoff_throttle.get(); _param_rwto_max_thr.get();
} }
case RunwayTakeoffState::CLAMPED_TO_RUNWAY: case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
return _takeoff_throttle.get(); return _param_rwto_max_thr.get();
default: default:
return tecsThrottle; return tecsThrottle;
@ -242,8 +242,8 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
float RunwayTakeoff::getMaxPitch(float max) float RunwayTakeoff::getMaxPitch(float max)
{ {
// use max pitch from parameter if set (> 0.1) // use max pitch from parameter if set (> 0.1)
if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) {
return _max_takeoff_pitch.get(); return _param_rwto_max_pitch.get();
} }
else { else {

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

@ -74,8 +74,8 @@ public:
RunwayTakeoffState getState() { return _state; } RunwayTakeoffState getState() { return _state; }
bool isInitialized() { return _initialized; } bool isInitialized() { return _initialized; }
bool runwayTakeoffEnabled() { return _runway_takeoff_enabled.get(); } bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); }
float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); } float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); }
float getInitYaw() { return _init_yaw; } float getInitYaw() { return _init_yaw; }
bool controlYaw(); bool controlYaw();
@ -102,16 +102,16 @@ private:
matrix::Vector2f _start_wp; matrix::Vector2f _start_wp;
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamBool<px4::params::RWTO_TKOFF>) _runway_takeoff_enabled, (ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
(ParamInt<px4::params::RWTO_HDG>) _heading_mode, (ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
(ParamFloat<px4::params::RWTO_NAV_ALT>) _nav_alt, (ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
(ParamFloat<px4::params::RWTO_MAX_THR>) _takeoff_throttle, (ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
(ParamFloat<px4::params::RWTO_PSP>) _runway_pitch_sp, (ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
(ParamFloat<px4::params::RWTO_MAX_PITCH>) _max_takeoff_pitch, (ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
(ParamFloat<px4::params::RWTO_MAX_ROLL>) _max_takeoff_roll, (ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _min_airspeed_scaling, (ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _airspeed_min, (ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _climbout_diff (ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
) )
}; };

Loading…
Cancel
Save