Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

fw pos ctrl: some incremental clean up of the class

Browse Source 
- documentation of units, params, returns, and descriptions for variables and methods
- rename ambiguous or erroneous state variables
- remove unused or unecessary input arguments to functions
- remove ugly header white space
This commit is contained in:
Thomas Stastny
2022-05-05 23:18:17 +02:00
committed by Silvan Fuhrer
parent 9863c24b40
commit 594a6d6e80
2 changed files with 354 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+46 -47
View File
@@ -67,7 +67,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
_pos_ctrl_landing_status_pub.advertise();
_tecs_status_pub.advertise();
_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);
_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
/* fetch initial parameter values */
parameters_update();
@@ -283,7 +283,7 @@ FixedwingPositionControl::airspeed_poll()
airspeed_valid = true;
_airspeed_last_valid = airspeed_validated.timestamp;
_time_airspeed_last_valid = airspeed_validated.timestamp;
_airspeed = airspeed_validated.calibrated_airspeed_m_s;
_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
@@ -291,7 +291,7 @@ FixedwingPositionControl::airspeed_poll()
} else {
// no airspeed updates for one second
if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
airspeed_valid = false;
}
}
@@ -321,7 +321,7 @@ FixedwingPositionControl::wind_poll()
} else {
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
}
}
@@ -330,21 +330,21 @@ FixedwingPositionControl::manual_control_setpoint_poll()
{
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_altitude = _manual_control_setpoint.x;
_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
/* Alternate stick allocation (similar concept as for multirotor systems:
* demanding up/down with the throttle stick, and move faster/break with the pitch one.
*/
_manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
_manual_control_setpoint_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
}
// send neutral setpoints if no update for 1 s
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
_manual_control_setpoint_altitude = 0.f;
_manual_control_setpoint_airspeed = 0.5f;
_manual_control_setpoint_for_height_rate = 0.f;
_manual_control_setpoint_for_airspeed = 0.5f;
}
}
@@ -393,17 +393,17 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
// neutral throttle corresponds to trim airspeed
if (_manual_control_setpoint_airspeed < 0.5f) {
if (_manual_control_setpoint_for_airspeed < 0.5f) {
// lower half of throttle is min to trim airspeed
altctrl_airspeed = _param_fw_airspd_min.get() +
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
_manual_control_setpoint_airspeed * 2;
_manual_control_setpoint_for_airspeed * 2;
} else {
// upper half of throttle is trim to max airspeed
altctrl_airspeed = _param_fw_airspd_trim.get() +
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
(_manual_control_setpoint_airspeed * 2 - 1);
(_manual_control_setpoint_for_airspeed * 2 - 1);
}
} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
@@ -460,15 +460,15 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva
airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());
// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
const bool outside_of_limits = _slew_rate_airspeed.getState() < airspeed_min_adjusted
|| _slew_rate_airspeed.getState() > _param_fw_airspd_max.get();
const bool outside_of_limits = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
|| _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) {
_slew_rate_airspeed.setForcedValue(airspeed_setpoint);
if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);
} else if (control_interval > FLT_EPSILON) {
// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, control_interval);
airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
}
return airspeed_setpoint;
@@ -707,14 +707,14 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual_control_setpoint_altitude > deadBand) {
if (_manual_control_setpoint_for_height_rate > deadBand) {
/* pitching down */
float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor;
height_rate_setpoint = pitch * _param_sinkrate_target.get();
} else if (_manual_control_setpoint_altitude < - deadBand) {
} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
/* pitching up */
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
const float climb_rate_target = _param_climbrate_target.get();
height_rate_setpoint = pitch * climb_rate_target;
@@ -882,7 +882,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
position_setpoint_s current_sp = pos_sp_curr;
move_position_setpoint_for_vtol_transition(current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp);
_position_sp_type = position_sp_type;
@@ -908,7 +908,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
@@ -974,8 +974,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
void
FixedwingPositionControl::control_auto_descend(const float control_interval)
{
// only control height rate
// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
// but not letting it drift too far away.
const float descend_rate = -0.5f;
@@ -1004,8 +1002,10 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
}
uint8_t
FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr)
FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
{
uint8_t position_sp_type = pos_sp_curr.type;
if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
}
@@ -1017,8 +1017,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = setpoint_type;
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@@ -1068,7 +1066,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
@@ -1145,6 +1142,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
}
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
@@ -1192,7 +1190,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
void
FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
{
float tecs_fw_thr_min;
float tecs_fw_thr_max;
@@ -1733,10 +1731,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
// all good, have valid terrain altitude
float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
terrain_alt = (_local_pos.ref_alt - terrain_vpos);
_t_alt_prev_valid = terrain_alt;
_time_last_t_alt = now;
_last_valid_terrain_alt_estimate = terrain_alt;
_last_time_terrain_alt_was_valid = now;
} else if (_time_last_t_alt == 0) {
} else if (_last_time_terrain_alt_was_valid == 0) {
// we have started landing phase but don't have valid terrain
// wait for some time, maybe we will soon get a valid estimate
// until then just use the altitude of the landing waypoint
@@ -1749,16 +1747,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
abort_landing(true);
}
} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
|| _land_noreturn_vertical) {
// use previous terrain estimate for some time and hope to recover
// if we are already flaring (land_noreturn_vertical) then just
// go with the old estimate
terrain_alt = _t_alt_prev_valid;
terrain_alt = _last_valid_terrain_alt_estimate;
} else {
// terrain alt was not valid for long time, abort landing
terrain_alt = _t_alt_prev_valid;
terrain_alt = _last_valid_terrain_alt_estimate;
abort_landing(true);
}
}
@@ -2009,7 +2007,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -2071,7 +2069,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -2520,7 +2518,7 @@ FixedwingPositionControl::reset_landing_state()
_time_started_landing = 0;
// reset terrain estimation relevant values
_time_last_t_alt = 0;
_last_time_terrain_alt_was_valid = 0;
_land_noreturn_horizontal = false;
_land_noreturn_vertical = false;
@@ -2588,24 +2586,25 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
// set this to transition airspeed to init tecs correctly
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
// some vtols fly without airspeed sensor
_asp_after_transition = _param_airspeed_trans;
_airspeed_after_transition = _param_airspeed_trans;
} else {
_asp_after_transition = _airspeed;
_airspeed_after_transition = _airspeed;
}
_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
_param_fw_airspd_max.get());
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_asp_after_transition += control_interval * 2.0f; // increase 2m/s
_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_asp_after_transition, _airspeed);
if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_airspeed_after_transition, _airspeed);
} else {
_was_in_transition = false;
_asp_after_transition = 0.0f;
_airspeed_after_transition = 0.0f;
}
}
}
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+308 -129
View File
File diff suppressed because it is too large Load Diff