fw_pos_control_l1: add takeoff minimum pitch parameter

- remove mavlink mechanism for setting minimum pitch

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1222,7 +1222,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
 					   _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
 					   _param_fw_thr_cruise.get(),
 					   _runway_takeoff.climbout(),
-					   radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _param_fw_p_lim_min.get())),
+					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())),
 					   tecs_status_s::TECS_MODE_TAKEOFF);
 
 		// assign values
@@ -1293,7 +1293,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
 							   takeoff_throttle,
 							   _param_fw_thr_cruise.get(),
 							   true,
-							   max(radians(pos_sp_curr.pitch_min), radians(10.0f)),
+							   radians(_takeoff_pitch_min.get()),
 							   tecs_status_s::TECS_MODE_TAKEOFF);
 
 				/* limit roll motion to ensure enough lift */
@@ -1320,7 +1320,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
 
 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
-			_att_sp.pitch_body = max(radians(pos_sp_curr.pitch_min), radians(10.0f));
+			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
 		}
 	}
 }

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -417,7 +417,9 @@ private:
 		(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
 		(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
 
-		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
+
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min
 
 	)
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -268,6 +268,18 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
  */
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 
+/**
+ * Minimum pitch during takeoff.
+ *
+ * @unit deg
+ * @min -5.0
+ * @max 30.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);
+
 /**
  *
  *

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -221,13 +221,12 @@ bool RunwayTakeoff::resetIntegrators()
  * the climbtout minimum pitch (parameter).
  * Otherwise use the minimum that is enforced generally (parameter).
  */
-float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
+float RunwayTakeoff::getMinPitch(float climbout_min, float min)
 {
 	if (_state < RunwayTakeoffState::FLY) {
-		return math::max(sp_min, climbout_min);
-	}
+		return climbout_min;
 
-	else {
+	} else {
 		return min;
 	}
 }

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -86,7 +86,7 @@ public:
 	float getYaw(float navigatorYaw);
 	float getThrottle(const hrt_abstime &now, float tecsThrottle);
 	bool resetIntegrators();
-	float getMinPitch(float sp_min, float climbout_min, float min);
+	float getMinPitch(float climbout_min, float min);
 	float getMaxPitch(float max);
 	matrix::Vector2f getStartWP();