fw landing: move more functionality to the landingslope class

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 &current_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 &current_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 &current_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 &current_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,