fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: some airspeed setpoint handling adaptions

Browse Source 
- introuce slew rate limiting of airspeed setpoint (with slew rate of 1 m/s/s)
- some refactoring and clean up

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-01-05 17:06:41 +01:00
committed by JaeyoungLim
parent 20a1e5f77c
commit 0a82025faf
2 changed files with 85 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+70 -67
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -333,7 +333,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
}
float
FixedwingPositionControl::get_demanded_airspeed()
FixedwingPositionControl::get_manual_airspeed_setpoint()
{
float altctrl_airspeed = 0;
@@ -355,50 +355,69 @@ FixedwingPositionControl::get_demanded_airspeed()
}
float
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed)
FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cruise_airspeed,
const Vector2f &ground_speed, float dt)
{
/*
* Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
*
* We don't know the stall speed of the aircraft, but assuming user defined
* minimum airspeed (FW_AIRSPD_MIN) is slightly larger than stall speed
* this is close enough.
*
* increase lift vector to balance additional weight in bank
* cos(bank angle) = W/L = 1/n
* n is the load factor
*
* lift is proportional to airspeed^2 so the increase in stall speed is
* Vsacc = Vs * sqrt(n)
*
*/
float adjusted_min_airspeed = _param_fw_airspd_min.get();
// overwrite internal setpoint (e.g. set prior through MAV_CMD_DO_CHANGE_SPEED) in case
// the current position_setpoint contains a valid airspeed setpoint
float airspeed_setpoint = _param_fw_airspd_trim.get();
if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
adjusted_min_airspeed = constrain(_param_fw_airspd_min.get() / sqrtf(cosf(_att_sp.roll_body)),
_param_fw_airspd_min.get(), _param_fw_airspd_max.get());
if (PX4_ISFINITE(pos_sp_cruise_airspeed) && pos_sp_cruise_airspeed > FLT_EPSILON) {
airspeed_setpoint = pos_sp_cruise_airspeed;
}
// groundspeed undershoot
// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
if (!_l1_control.circle_mode()) {
/*
* This error value ensures that a plane (as long as its throttle capability is
* not exceeded) travels towards a waypoint (and is not pushed more and more away
* by wind). Not countering this would lead to a fly-away.
* by wind). Not countering this would lead to a fly-away. Only non-zero in presence
* of sufficient wind. "minimum ground speed undershoot".
*/
const float ground_speed_body = _body_velocity(0);
if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body;
}
}
// add minimum ground speed undershoot (only non-zero in presence of sufficient wind)
// sanity check: limit to range
return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get());
float airspeed_min_adjusted = _param_fw_airspd_min.get();
/*
* Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
*
* Increase lift vector to balance additional weight in bank
* cos(bank angle) = W/L = 1/n, n is the load factor
*
* lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n)
*/
if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() / sqrtf(cosf(_att_sp.roll_body)),
airspeed_min_adjusted, _param_fw_airspd_max.get());
}
// constrain setpoint
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 = _last_airspeed_setpoint < airspeed_min_adjusted
|| _last_airspeed_setpoint > _param_fw_airspd_max.get();
if (!PX4_ISFINITE(_last_airspeed_setpoint) || outside_of_limits) {
_last_airspeed_setpoint = airspeed_setpoint;
} else if (dt > FLT_EPSILON) {
// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
airspeed_setpoint = constrain(airspeed_setpoint, _last_airspeed_setpoint - ASPD_SP_SLEW_RATE * dt,
_last_airspeed_setpoint + ASPD_SP_SLEW_RATE * dt);
_last_airspeed_setpoint = airspeed_setpoint;
}
return airspeed_setpoint;
}
void
FixedwingPositionControl::tecs_status_publish()
{
@@ -784,19 +803,19 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
break;
case position_setpoint_s::SETPOINT_TYPE_POSITION:
control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
control_auto_velocity(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
break;
case position_setpoint_s::SETPOINT_TYPE_LAND:
control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
control_auto_landing(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
@@ -1000,7 +1019,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
}
void
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
const float acc_rad = _l1_control.switch_distance(500.0f);
@@ -1024,15 +1043,6 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
prev_wp(1) = pos_sp_curr.lon;
}
float mission_airspeed = _param_fw_airspd_trim.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
mission_airspeed = pos_sp_curr.cruising_speed;
}
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
@@ -1098,7 +1108,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
tecs_update_pitch_throttle(now, position_sp_alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1109,17 +1119,9 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
}
void
FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
float mission_airspeed = _param_fw_airspd_trim.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
mission_airspeed = pos_sp_curr.cruising_speed;
}
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
@@ -1157,7 +1159,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const Ve
_att_sp.yaw_body = _yaw;
tecs_update_pitch_throttle(now, position_sp_alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1170,7 +1172,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const Ve
}
void
FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next)
{
@@ -1193,12 +1195,12 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
prev_wp(1) = pos_sp_curr.lon;
}
float mission_airspeed = _param_fw_airspd_trim.get();
float airspeed_sp = -1.f;
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
pos_sp_curr.cruising_speed > FLT_EPSILON) {
mission_airspeed = pos_sp_curr.cruising_speed;
airspeed_sp = pos_sp_curr.cruising_speed;
}
float tecs_fw_thr_min;
@@ -1247,7 +1249,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
// landing airspeed and potentially tighter altitude control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
_att_sp.apply_flaps = true;
}
@@ -1270,7 +1272,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
}
tecs_update_pitch_throttle(now, alt_sp,
calculate_target_airspeed(mission_airspeed, ground_speed),
get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1340,7 +1342,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed),
get_auto_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
dt),
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
@@ -1425,7 +1428,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
} else {
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed),
get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
@@ -1450,7 +1453,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
}
void
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
/* current waypoint (the one currently heading for) */
@@ -1643,7 +1646,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land, ground_speed),
get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt),
radians(_param_fw_lnd_fl_pmin.get()),
radians(_param_fw_lnd_fl_pmax.get()),
0.0f,
@@ -1713,7 +1716,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
tecs_update_pitch_throttle(now, altitude_desired,
calculate_target_airspeed(airspeed_approach, ground_speed),
get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
@@ -1732,7 +1735,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
_control_position_last_called = now;
/* Get demanded airspeed */
float altctrl_airspeed = get_demanded_airspeed();
float altctrl_airspeed = get_manual_airspeed_setpoint();
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
@@ -1819,7 +1822,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
}
tecs_update_pitch_throttle(now, altitude_sp_amsl,
get_demanded_airspeed(),
get_manual_airspeed_setpoint(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+15 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -112,6 +112,7 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
@@ -253,6 +254,8 @@ private:
float _manual_control_setpoint_altitude{0.0f};
float _manual_control_setpoint_airspeed{0.0f};
float _last_airspeed_setpoint{0.f};
hrt_abstime _time_in_fixed_bank_loiter{0};
ECL_L1_Pos_Controller _l1_control;
@@ -327,17 +330,21 @@ private:
void control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
void control_auto_descend(const hrt_abstime &now);
void control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
void control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
void control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
void control_auto_velocity(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
void control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_takeoff(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
void control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
@@ -346,8 +353,9 @@ private:
float get_tecs_pitch();
float get_tecs_thrust();
float get_demanded_airspeed();
float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed);
float get_manual_airspeed_setpoint();
float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed,
const Vector2f &ground_speed, float dt);
void reset_takeoff_state(bool force = false);
void reset_landing_state();
@@ -426,7 +434,6 @@ private:
(ParamBool<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,