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

Fixed-wing autoland: 1) The landing configuration (flaps, different airspeed) is now already set during the loiter down instead of at the start of the landing approach. This is done to avoid any mode changes (which can cause altitude/airspeed jumps) so close to the gorund. 2) A scaling factor for the TECS throttle time constant was added which allows tighter throttle control during the landing (i.e. close to the ground) than high up in the air

Browse Source
This commit is contained in:
Philipp Oettershagen 2018-06-16 12:26:21 +02:00 committed by Daniel Agar
parent 790356ef6d
commit 4c4f585ad5
3 changed files with 44 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -76,6 +76,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER");
_parameter_handles.land_airspeed_scale = param_find("FW_LND_AIRSPD_SC");
_parameter_handles.land_throtTC_scale = param_find("FW_LND_THRTC_SC");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
@ -148,6 +149,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale));
// VTOL parameter VTOL_TYPE
if (_parameter_handles.vtol_type != PARAM_INVALID) {
@ -195,12 +197,12 @@ FixedwingPositionControl::parameters_update()
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
if (param_get(_parameter_handles.time_const, &v) == PX4_OK) {
_tecs.set_time_const(v);
if (param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)) == PX4_OK) {
_tecs.set_time_const_throt(_parameters.time_const_throt);
}
if (param_get(_parameter_handles.time_const_throt, &v) == PX4_OK) {
_tecs.set_time_const_throt(v);
if (param_get(_parameter_handles.time_const, &v) == PX4_OK) {
_tecs.set_time_const(v);
}
if (param_get(_parameter_handles.min_sink_rate, &v) == PX4_OK) {
@ -694,7 +696,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
bool
FixedwingPositionControl::control_position(const Vector2f &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_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
{
float dt = 0.01f;
@ -770,8 +772,9 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_time_const_throt(_parameters.time_const_throt);
/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
@ -844,6 +847,15 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
float alt_sp = pos_sp_curr.alt;
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter throttle control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_time_const_throt(_parameters.land_throtTC_scale * _parameters.time_const_throt);
mission_airspeed = _parameters.land_airspeed_scale * _parameters.airspeed_min;
_att_sp.apply_flaps = true;
}
if (in_takeoff_situation()) {
alt_sp = max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
@ -858,6 +870,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
// continue straight until vehicle has sufficient altitude
_att_sp.roll_body = 0.0f;
}
_tecs.set_time_const_throt(_parameters.land_throtTC_scale * _parameters.time_const_throt);
}
tecs_update_pitch_throttle(alt_sp,
@ -1322,6 +1336,9 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
// if they have been enabled using the corresponding parameter
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
// Enable tighter throttle control for landings
_tecs.set_time_const_throt(_parameters.land_throtTC_scale * _parameters.time_const_throt);
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
_time_started_landing = hrt_absolute_time();
@ -1709,7 +1726,7 @@ FixedwingPositionControl::run()
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current)) {
if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) {
_att_sp.timestamp = hrt_absolute_time();
// add attitude setpoint offsets

5
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -267,6 +267,7 @@ private:
float max_climb_rate;
float max_sink_rate;
float speed_weight;
float time_const_throt;
float airspeed_min;
float airspeed_trim;
@ -294,6 +295,7 @@ private:
float land_flare_pitch_max_deg;
int32_t land_use_terrain_estimate;
float land_airspeed_scale;
float land_throtTC_scale;
// VTOL
float airspeed_trans;
@ -357,6 +359,7 @@ private:
param_t land_flare_pitch_max_deg;
param_t land_use_terrain_estimate;
param_t land_airspeed_scale;
param_t land_throtTC_scale;
param_t vtol_type;
} _parameter_handles {}; ///< handles for interesting parameters */
@ -418,7 +421,7 @@ private:
bool update_desired_altitude(float dt);
bool control_position(const Vector2f &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_curr, const position_setpoint_s &pos_sp_next);
void control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,

16
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -374,6 +374,22 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
*/
PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);
/**
* Throttle time constant factor for landing
*
* Set this parameter to <1.0 to make the TECS throttle loop react faster during
* landing than during normal flight (i.e. giving efficiency and low motor wear at
* high altitudes but control accuracy during landing). During landing, the TECS
* throttle time constant (FW_T_THRO_CONST) is multiplied by this value.
*
* @unit
* @min 0.2
* @max 1.0
* @increment 0.1
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
/*