fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 07:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: centralise airspeed setpoint adaptions for auto flight in single function

Browse Source 
Adapt the airspeed setpoint by:
- speed mode (ECO/NORMAL/DASH --> airspeed MIN, TRIM, MAX)
- if position setpoint contains valid airspeed setpoint use that instead
- increase setpoint if groundspeed gets low
- increase setpoint if close to MIN and banking (compensate higher load factor)

There is a slew rate limiting in place that allows max 1m/s/s rate of change of
the airspeed setpoint.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-25 09:50:28 +01:00
parent af394e6ebd
commit 9f1f8348f5
2 changed files with 74 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+64 -53
View File
@@ -354,24 +354,51 @@ FixedwingPositionControl::get_demanded_airspeed()
return altctrl_airspeed;
}
float
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed)
FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed,
const Vector2f &ground_speed, float dt)
{
float airspeed_setpoint = _param_fw_airspd_trim.get();
if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO) {
airspeed_setpoint = _param_fw_airspd_min.get();
} else if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_DASH) {
airspeed_setpoint = _param_fw_airspd_max.get();
}
// Adapt cruise airspeed setpoint if given from other source (e.g. position setpoint)
if (PX4_ISFINITE(pos_sp_cru_airspeed) && pos_sp_cru_airspeed > 0.1f) {
airspeed_setpoint = constrain(pos_sp_cru_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
}
// 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. 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_setpoint += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
}
}
// Constrain cruise airspeed to be in valid and safe range
/*
* 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)
* 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();
if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
@@ -380,23 +407,21 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
_param_fw_airspd_min.get(), _param_fw_airspd_max.get());
}
// groundspeed undershoot
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.
*/
const float ground_speed_body = _body_velocity(0);
// constrain airspeed setpoint changes with slew rate of 1 m/s/s
const float airspeed_setpoint_slew_rate = 1.f;
if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
}
// initialize to current airspeed setpoint
if (!PX4_ISFINITE(_last_airspeed_setpoint)) {
_last_airspeed_setpoint = airspeed_setpoint;
}
// 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());
if (dt > FLT_EPSILON) {
airspeed_setpoint = constrain(airspeed_setpoint, _last_airspeed_setpoint - airspeed_setpoint_slew_rate * dt,
_last_airspeed_setpoint + airspeed_setpoint_slew_rate * dt);
_last_airspeed_setpoint = airspeed_setpoint;
}
return constrain(airspeed_setpoint, adjusted_min_airspeed, _param_fw_airspd_max.get());
}
void
@@ -835,15 +860,15 @@ 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_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:
@@ -1033,7 +1058,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);
@@ -1057,15 +1082,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;
@@ -1131,7 +1147,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_cruise_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,
@@ -1142,7 +1158,7 @@ FixedwingPositionControl::control_auto_position(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)
{
@@ -1165,13 +1181,7 @@ 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();
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
mission_airspeed = pos_sp_curr.cruising_speed;
}
float mission_airspeed = pos_sp_curr.cruising_speed;
float tecs_fw_thr_min;
float tecs_fw_thr_max;
@@ -1242,7 +1252,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_cruise_airspeed_setpoint(now, mission_airspeed, ground_speed, dt),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1312,7 +1322,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_cruise_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(),
@@ -1397,7 +1408,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_cruise_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(),
@@ -1422,7 +1433,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) */
@@ -1615,7 +1626,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_cruise_airspeed_setpoint(now, airspeed_land, ground_speed, dt),
radians(_param_fw_lnd_fl_pmin.get()),
radians(_param_fw_lnd_fl_pmax.get()),
0.0f,
@@ -1685,7 +1696,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_cruise_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(),
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+10 -4
View File
@@ -296,6 +296,8 @@ private:
FW_WIND_MODE _fw_wind_mode_detected_prev{FW_WIND_MODE_NORMAL};
float _last_airspeed_setpoint{NAN};
param_t _param_handle_airspeed_trans{PARAM_INVALID};
float _param_airspeed_trans{NAN};
@@ -353,15 +355,18 @@ 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_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);
@@ -371,7 +376,8 @@ private:
float get_tecs_thrust();
float get_demanded_airspeed();
float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed);
float get_cruise_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();