make flare pitch angle a param

2 files changed, 86 insertions, 62 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 e8e316064..c0c238ba9 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
@@ -237,6 +237,8 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
+		float land_flare_pitch_min_deg;
+		float land_flare_pitch_max_deg;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -281,6 +283,8 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
+		param_t land_flare_pitch_min_deg;
+		param_t land_flare_pitch_max_deg;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -465,6 +469,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
+	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -563,6 +569,8 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
+	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
+	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -958,7 +966,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* 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_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_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;
@@ -1008,9 +1015,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
-						flare_pitch_angle_rad, math::radians(15.0f),
+						 math::radians(_parameters.land_flare_pitch_min_deg),
+						 math::radians(_parameters.land_flare_pitch_max_deg),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad,
+						false,  math::radians(_parameters.land_flare_pitch_min_deg),
 						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
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 890ab3bad..f2e8c422f 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
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 /**
  * Throttle limit max
  *
- * This is the maximum throttle % that can be used by the controller. 
- * For overpowered aircraft, this should be reduced to a value that 
+ * This is the maximum throttle % that can be used by the controller.
+ * For overpowered aircraft, this should be reduced to a value that
  * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
  *
  * @group L1 Control
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 /**
  * Throttle limit min
  *
- * This is the minimum throttle % that can be used by the controller. 
- * For electric aircraft this will normally be set to zero, but can be set 
- * to a small non-zero value if a folding prop is fitted to prevent the 
- * prop from folding and unfolding repeatedly in-flight or to provide 
+ * This is the minimum throttle % that can be used by the controller.
+ * For electric aircraft this will normally be set to zero, but can be set
+ * to a small non-zero value if a folding prop is fitted to prevent the
+ * prop from folding and unfolding repeatedly in-flight or to provide
  * some aerodynamic drag from a turning prop to improve the descent rate.
  *
  * For aircraft with internal combustion engine this parameter should be set
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 /**
  * Throttle limit value before flare
  *
- * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * This throttle value will be set as throttle limit at FW_LND_TLALT,
  * before arcraft will flare.
  *
  * @group L1 Control
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
 /**
  * Maximum climb rate
  *
- * This is the best climb rate that the aircraft can achieve with 
- * the throttle set to THR_MAX and the airspeed set to the 
- * default value. For electric aircraft make sure this number can be 
- * achieved towards the end of flight when the battery voltage has reduced. 
- * The setting of this parameter can be checked by commanding a positive 
- * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
- * required to climb is close to THR_MAX and the aircraft is maintaining 
- * airspeed, then this parameter is set correctly. If the airspeed starts 
- * to reduce, then the parameter is set to high, and if the throttle 
- * demand required to climb and maintain speed is noticeably less than 
- * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * This is the best climb rate that the aircraft can achieve with
+ * the throttle set to THR_MAX and the airspeed set to the
+ * default value. For electric aircraft make sure this number can be
+ * achieved towards the end of flight when the battery voltage has reduced.
+ * The setting of this parameter can be checked by commanding a positive
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle
+ * required to climb is close to THR_MAX and the aircraft is maintaining
+ * airspeed, then this parameter is set correctly. If the airspeed starts
+ * to reduce, then the parameter is set to high, and if the throttle
+ * demand required to climb and maintain speed is noticeably less than
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
  * FW_THR_MAX reduced.
  *
  * @group L1 Control
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 /**
  * Minimum descent rate
  *
- * This is the sink rate of the aircraft with the throttle 
- * set to THR_MIN and flown at the same airspeed as used 
+ * This is the sink rate of the aircraft with the throttle
+ * set to THR_MIN and flown at the same airspeed as used
  * to measure FW_T_CLMB_MAX.
  *
  * @group Fixed Wing TECS
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 /**
  * Maximum descent rate
  *
- * This sets the maximum descent rate that the controller will use. 
- * If this value is too large, the aircraft can over-speed on descent. 
- * This should be set to a value that can be achieved without 
- * exceeding the lower pitch angle limit and without over-speeding 
+ * This sets the maximum descent rate that the controller will use.
+ * If this value is too large, the aircraft can over-speed on descent.
+ * This should be set to a value that can be achieved without
+ * exceeding the lower pitch angle limit and without over-speeding
  * the aircraft.
  *
  * @group Fixed Wing TECS
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 /**
  * TECS time constant
  *
- * This is the time constant of the TECS control algorithm (in seconds). 
+ * This is the time constant of the TECS control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 /**
  * TECS Throttle time constant
  *
- * This is the time constant of the TECS throttle control algorithm (in seconds). 
+ * This is the time constant of the TECS throttle control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
 /**
  * Throttle damping factor
  *
- * This is the damping gain for the throttle demand loop. 
+ * This is the damping gain for the throttle demand loop.
  * Increase to add damping to correct for oscillations in speed and height.
  *
  * @group Fixed Wing TECS
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 /**
  * Integrator gain
  *
- * This is the integrator gain on the control loop. 
- * Increasing this gain increases the speed at which speed 
- * and height offsets are trimmed out, but reduces damping and 
+ * This is the integrator gain on the control loop.
+ * Increasing this gain increases the speed at which speed
+ * and height offsets are trimmed out, but reduces damping and
  * increases overshoot.
  *
  * @group Fixed Wing TECS
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
  * Maximum vertical acceleration
  *
  * This is the maximum vertical acceleration (in metres/second square)
- * either up or down that the controller will use to correct speed 
- * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
- * allows for reasonably aggressive pitch changes if required to recover 
+ * either up or down that the controller will use to correct speed
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
+ * allows for reasonably aggressive pitch changes if required to recover
  * from under-speed conditions.
  *
  * @group Fixed Wing TECS
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 /**
  * Complementary filter "omega" parameter for height
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse vertical acceleration and barometric height to obtain 
- * an estimate of height rate and height. Increasing this frequency weights 
- * the solution more towards use of the barometer, whilst reducing it weights 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse vertical acceleration and barometric height to obtain
+ * an estimate of height rate and height. Increasing this frequency weights
+ * the solution more towards use of the barometer, whilst reducing it weights
  * the solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 /**
  * Complementary filter "omega" parameter for speed
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an
  * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * more towards use of the arispeed sensor, whilst reducing it weights the
  * solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 /**
  * Roll -> Throttle feedforward
  *
- * Increasing this gain turn increases the amount of throttle that will 
- * be used to compensate for the additional drag created by turning. 
- * Ideally this should be set to  approximately 10 x the extra sink rate 
- * in m/s created by a 45 degree bank turn. Increase this gain if 
- * the aircraft initially loses energy in turns and reduce if the 
- * aircraft initially gains energy in turns. Efficient high aspect-ratio 
- * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * Increasing this gain turn increases the amount of throttle that will
+ * be used to compensate for the additional drag created by turning.
+ * Ideally this should be set to  approximately 10 x the extra sink rate
+ * in m/s created by a 45 degree bank turn. Increase this gain if
+ * the aircraft initially loses energy in turns and reduce if the
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio
+ * aircraft (eg powered sailplanes) can use a lower value, whereas
  * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
  *
  * @group Fixed Wing TECS
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 /**
  * Speed <--> Altitude priority
  *
- * This parameter adjusts the amount of weighting that the pitch control 
- * applies to speed vs height errors. Setting it to 0.0 will cause the 
- * pitch control to control height and ignore speed errors. This will 
- * normally improve height accuracy but give larger airspeed errors. 
- * Setting it to 2.0 will cause the pitch control loop to control speed 
- * and ignore height errors. This will normally reduce airspeed errors, 
- * but give larger height errors. The default value of 1.0 allows the pitch 
- * control to simultaneously control height and speed. 
- * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * This parameter adjusts the amount of weighting that the pitch control
+ * applies to speed vs height errors. Setting it to 0.0 will cause the
+ * pitch control to control height and ignore speed errors. This will
+ * normally improve height accuracy but give larger airspeed errors.
+ * Setting it to 2.0 will cause the pitch control loop to control speed
+ * and ignore height errors. This will normally reduce airspeed errors,
+ * but give larger height errors. The default value of 1.0 allows the pitch
+ * control to simultaneously control height and speed.
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will
  * adjust its pitch angle to maintain airspeed, ignoring changes in height).
  *
  * @group Fixed Wing TECS
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 /**
  * Pitch damping factor
  *
- * This is the damping gain for the pitch demand loop. Increase to add 
- * damping to correct for oscillations in height. The default value of 0.0 
- * will work well provided the pitch to servo controller has been tuned 
+ * This is the damping gain for the pitch demand loop. Increase to add
+ * damping to correct for oscillations in height. The default value of 0.0
+ * will work well provided the pitch to servo controller has been tuned
  * properly.
  *
  * @group Fixed Wing TECS
@@ -414,3 +414,19 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
  * @group L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);