From 4c4f585ad568bb7db9bf631d87f7dcd508065060 Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Sat, 16 Jun 2018 12:26:21 +0200 Subject: [PATCH] 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 --- .../FixedwingPositionControl.cpp | 31 ++++++++++++++----- .../FixedwingPositionControl.hpp | 5 ++- .../fw_pos_control_l1_params.c | 16 ++++++++++ 3 files changed, 44 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 5a29bc480f..f0d446085e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index ddf6ed5e02..584007876e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index ab7f465b24..b001bb1a24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -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); + /*