diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.cpp | 6 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.h | 1 |
3 files changed, 12 insertions, 6 deletions
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 ae0f8c0ea..2be25517d 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 @@ -845,8 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; @@ -876,7 +875,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = _mission_item_triplet.current.altitude + landingslope.H0() * expf(-math::max(0.0f, landingslope.flare_length() - wp_distance)/landingslope.flare_constant()) - landingslope.H1_virt(); + float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) @@ -887,9 +886,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _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, + false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, - flare_angle_rad, math::radians(15.0f)); + flare_pitch_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -904,7 +903,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, + false, flare_pitch_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: 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); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index ecf51b740..a2d8525b9 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -47,6 +47,7 @@ #include <errno.h> #include <math.h> #include <unistd.h> +#include <mathlib/mathlib.h> void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, @@ -76,3 +77,8 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } +float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude) +{ + return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index f855b796f..b77252e6e 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,6 +63,7 @@ public: ~Landingslope() {} float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getFlarceCurveAltitude(float wp_distance, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, |