From bdbe64026b0663489f31841fbc894befd839f732 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 13:10:14 +0100 Subject: introduce experimental flare trajectory --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 94 ++++++++++++++-------- 1 file changed, 61 insertions(+), 33 deletions(-) (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3aa3acb1a..9de6f8366 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -292,7 +295,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); /** * Control position. @@ -353,7 +356,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -374,7 +380,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -648,9 +654,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } bool @@ -762,7 +768,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -771,15 +778,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) //set target_bearing in first occurrence + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn = true; + land_noreturn_horizontal = true; } else { @@ -791,6 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { @@ -806,13 +814,19 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float landingslope_length = _parameters.land_slope_length; - float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") - float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - - if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + + if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -820,18 +834,34 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; + + if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, throttle_max, throttle_land, - math::radians(flare_angle_rad), math::radians(15.0f)); + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - warnx("Landing: land with minimal speed"); + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; } else if (wp_distance < L_wp_distance) { @@ -841,26 +871,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally - * if current position is below altitude at L, climb to altitude of L */ + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { /* continue horizontally */ - altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); - } else { - /* climb to L_altitude */ - altitude_desired = L_altitude; - warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -957,7 +983,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { -- cgit v1.2.3