FW position control add takeoff minimum pitch

msg/position_setpoint.msg

@@ -39,7 +39,6 @@ bool yawspeed_valid		# true if yawspeed setpoint valid
 
 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.hpp

@@ -168,10 +168,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	fw_pos_ctrl_status_s		_fw_pos_ctrl_status {};		///< navigation capabilities */
 	manual_control_setpoint_s	_manual {};			///< r/c channel data */
-	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items */
-	vehicle_attitude_s	_att {};			///< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint */
-	vehicle_command_s		_vehicle_command {};		///< vehicle commands */
 	vehicle_control_mode_s		_control_mode {};		///< control mode */
 	vehicle_global_position_s	_global_pos {};			///< global vehicle position */
 	vehicle_local_position_s	_local_pos {};			///< vehicle local position */
@@ -191,8 +188,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	float	_althold_epv{0.0f};				///< the position estimate accuracy when engaging alt hold */
 	bool	_was_in_deadband{false};			///< wether the last stick input was in althold deadband */
 
-	position_setpoint_s _hdg_hold_prev_wp {};		///< position where heading hold started */
-	position_setpoint_s _hdg_hold_curr_wp {};		///< position to which heading hold flies */
+	position_setpoint_s _pos_sp_prev {};			///< position where heading hold started */
+	position_setpoint_s _pos_sp_curr {};			///< position to which heading hold flies */
 
 	hrt_abstime _control_position_last_called{0};		///< last call of control_position  */
 
@@ -235,10 +232,13 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	float _groundspeed_undershoot{0.0f};			///< ground speed error to min. speed in m/s
 
 	Dcmf _R_nb;				///< current attitude
+
 	float _roll{0.0f};
 	float _pitch{0.0f};
 	float _yaw{0.0f};
 
+	float _yawspeed{0.0f};
+
 	bool _reinitialize_tecs{true};				///< indicates if the TECS states should be reinitialized (used for VTOL)
 	bool _is_tecs_running{false};
 	hrt_abstime _last_tecs_update{0};
@@ -359,6 +359,9 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		param_t vtol_type;
 	} _parameter_handles {};				///< handles for interesting parameters */
 
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min
+	)
 
 	/**
 	 * Update our local parameter cache.
@@ -415,28 +418,24 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	 */
 	bool		update_desired_altitude(float dt);
 
-	bool		control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-					 const position_setpoint_s &pos_sp_curr);
-	void		control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-					const position_setpoint_s &pos_sp_curr);
-	void		control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-					const position_setpoint_s &pos_sp_curr);
+	bool		control_position(const Vector2f &curr_pos, const Vector2f &ground_speed);
+	void		control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed);
+	void		control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed);
 
 	float		get_tecs_pitch();
 	float		get_tecs_thrust();
 
 	float		get_demanded_airspeed();
 	float		calculate_target_airspeed(float airspeed_demand);
-	void		calculate_gndspeed_undershoot(const Vector2f &curr_pos, const Vector2f &ground_speed,
-			const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	void		calculate_gndspeed_undershoot(const Vector2f &curr_pos, const Vector2f &ground_speed);
 
 	/**
 	 * Handle incoming vehicle commands
 	 */
-	void		handle_command();
+	void		handle_command(const vehicle_command_s &vcmd);
 
 	void		reset_takeoff_state();
-	void		reset_landing_state();
+	void		reset_landing_state(const uint8_t pos_sp_type);
 
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -256,6 +256,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

@@ -223,13 +223,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

@@ -84,7 +84,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 	float getYaw(float navigatorYaw);
 	float getThrottle(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

@@ -1295,13 +1295,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;
@@ -1332,7 +1325,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_NAV_TAKEOFF:
 			mission_item->nav_cmd = NAV_CMD_TAKEOFF;
-			mission_item->pitch_min = mavlink_mission_item->param1;
 			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
 			break;
 
@@ -1598,7 +1590,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 = mission_item->yaw * M_RAD_TO_DEG_F;
 			break;
 

src/modules/navigator/mission.cpp

@@ -554,11 +554,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
@@ -585,10 +580,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 */

src/modules/navigator/mission_block.cpp

@@ -523,9 +523,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;
@@ -606,7 +603,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;
 
@@ -619,7 +616,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

@@ -95,7 +95,7 @@ class MissionBlock : public NavigatorMode
 	/**
 	 * 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

@@ -139,7 +139,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 */

src/modules/sdlog2/sdlog2.c

@@ -1810,7 +1810,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 					log_msg.body.log_GPSP.type = buf.triplet.current.type;
 					log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
 					log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-					log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 					LOGBUFFER_WRITE_AND_COUNT(GPSP);
 				}
 			}

src/modules/sdlog2/sdlog2_messages.h

@@ -255,7 +255,6 @@ struct log_GPSP_s {
 	uint8_t type;
 	float loiter_radius;
 	int8_t loiter_direction;
-	float pitch_min;
 };
 
 /* --- ESC - ESC STATE --- */
@@ -609,8 +608,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "BffffffLLHhB",	"ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
-	LOG_FORMAT(GPSP, "LLffBfbf",		"Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GPSP, "LLffBfbf",		"Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir"),
+	LOG_FORMAT(ESC, "HBBBHHffiff",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "fffffffB",		"V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"),
 	LOG_FORMAT(DIST, "BBBff",			"Id,Type,Orientation,Distance,Covariance"),