fw_pos_control_l1: add takeoff minimum pitch parameter

- remove mavlink mechanism for setting minimum pitch

integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan

@@ -22,7 +22,7 @@
                 "doJumpId": 1,
                 "frame": 3,
                 "params": [
-                    15,
+                    0,
                     0,
                     0,
                     null,

integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan

@@ -22,7 +22,7 @@
                 "doJumpId": 1,
                 "frame": 3,
                 "params": [
-                    15,
+                    0,
                     0,
                     0,
                     null,

integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan

@@ -22,7 +22,7 @@
                 "doJumpId": 1,
                 "frame": 3,
                 "params": [
-                    15,
+                    0,
                     0,
                     0,
                     null,

integrationtests/python_src/px4_it/mavros/missions/avoidance.plan

@@ -22,7 +22,7 @@
                 "doJumpId": 1,
                 "frame": 3,
                 "params": [
-                    15,
+                    0,
                     0,
                     0,
                     null,

msg/position_setpoint.msg

@@ -42,7 +42,6 @@ int8 landing_gear		# landing gear: see definition of the states in landing_gear.
 
 float32 loiter_radius		# loiter radius (only for fixed wing), in m
 int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
-float32 pitch_min		# minimal pitch angle for fixed wing takeoff waypoints
 
 float32 a_x			# acceleration x setpoint
 float32 a_y			# acceleration y setpoint

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();
 

src/modules/mavlink/mavlink_mission.cpp

@@ -1305,13 +1305,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->altitude_is_relative = true;
 		}
 
-		/* this field is shared with pitch_min (and circle_radius for geofence) in memory and
-		 * exclusive in the MAVLink spec. Set it to 0 first
-		 * and then set minimum pitch later only for the
-		 * corresponding item
-		 */
-		mission_item->time_inside = 0.0f;
-
 		switch (mavlink_mission_item->command) {
 		case MAV_CMD_NAV_WAYPOINT:
 			mission_item->nav_cmd = NAV_CMD_WAYPOINT;
@@ -1345,9 +1338,17 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			break;
 
 		case MAV_CMD_NAV_TAKEOFF:
+
+			// reject takeoff item if minimum pitch (parameter 1) is set
+			if (PX4_ISFINITE(mavlink_mission_item->param1) && (fabsf(mavlink_mission_item->param1) > FLT_EPSILON)) {
+				_mavlink->send_statustext_critical("Takeoff rejected, remove deprecated minimum pitch");
+				return MAV_MISSION_INVALID_PARAM1;
+
+			} else {
 				mission_item->nav_cmd = NAV_CMD_TAKEOFF;
-			mission_item->pitch_min = mavlink_mission_item->param1;
 				mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
+			}
+
 			break;
 
 		case MAV_CMD_NAV_LOITER_TO_ALT:
@@ -1652,7 +1653,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 			break;
 
 		case NAV_CMD_TAKEOFF:
-			mavlink_mission_item->param1 = mission_item->pitch_min;
 			mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
 			break;
 

src/modules/navigator/mission.cpp

@@ -752,11 +752,6 @@ Mission::set_mission_items()
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
 					_mission_item.yaw = NAN;
-					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
-					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
-					 * Note also that resetting time_inside to zero will cause pitch_min to be zero as well.
-					 */
-					_mission_item.time_inside = 0.0f;
 
 				} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
 					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
@@ -783,10 +778,6 @@ Mission::set_mission_items()
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
 					_mission_item.yaw = NAN;
-					/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
-					 * since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
-					 */
-					_mission_item.time_inside = 0.0f;
 				}
 
 				/* if we just did a VTOL takeoff, prepare transition */
@@ -1864,7 +1855,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
 		(p1->loiter_direction == p2->loiter_direction) &&
-		(fabsf(p1->pitch_min - p2->pitch_min) < FLT_EPSILON) &&
 		(fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) &&
 		(fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) &&
 		(fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) &&

src/modules/navigator/mission_block.cpp

@@ -582,9 +582,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 		} else {
 			sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
-
-			// set pitch and ensure that the hold time is zero
-			sp->pitch_min = item.pitch_min;
 		}
 
 		break;
@@ -667,7 +664,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 }
 
 void
-MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
+MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 {
 	item->nav_cmd = NAV_CMD_TAKEOFF;
 
@@ -680,7 +677,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
 	item->altitude_is_relative = false;
 
 	item->loiter_radius = _navigator->get_loiter_radius();
-	item->pitch_min = min_pitch;
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
 }

src/modules/navigator/mission_block.h

@@ -116,7 +116,7 @@ protected:
 	/**
 	 * Set a takeoff mission item
 	 */
-	void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch = 0.0f);
+	void set_takeoff_item(struct mission_item_s *item, float abs_altitude);
 
 	/**
 	 * Set a land mission item

src/modules/navigator/navigation.h

@@ -153,7 +153,6 @@ struct mission_item_s {
 		struct {
 			union {
 				float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
-				float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 				float circle_radius;		/**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
 			};
 			float acceptance_radius;		/**< default radius in which the mission is accepted as reached in meters */