[WIP] FW navigation first order hold move to position controller

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -440,7 +440,10 @@ FixedwingPositionControl::position_setpoint_triplet_poll()
 	orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
 
 	if (pos_sp_triplet_updated) {
-		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+		if (orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet) == PX4_OK) {
+			// reset the altitude foh (first order hold) logic
+			_min_current_sp_distance_xy = FLT_MAX;
+		}
 	}
 }
 
@@ -774,9 +777,7 @@ bool
 FixedwingPositionControl::in_takeoff_situation()
 {
 	// in air for < 10s
-	const hrt_abstime delta_takeoff = 10_s;
-
-	return (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff)
+	return (hrt_elapsed_time(&_time_went_in_air) < 10_s)
 	       && (_global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff);
 }
 
@@ -915,18 +916,91 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			mission_throttle = pos_sp_curr.cruising_throttle;
 		}
 
-		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		const float acc_rad = _l1_control.switch_distance(500.0f);
+
+
+		uint8_t position_sp_type = pos_sp_curr.type;
+
+		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+			// TAKEOFF: handle like a regular POSITION setpoint if already flying
+
+			const bool airspeed_min = (_airspeed >= _parameters.airspeed_min);
+
+			if (!in_takeoff_situation() && airspeed_min) {
+				// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
+				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+			}
+
+		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+			// POSITION: achieve position setpoint altitude via loiter
+
+			float dist_xy = -1.0f;
+			float dist_z = -1.0f;
+
+			const float dist = get_distance_to_point_global_wgs84(
+						   pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_curr.alt,
+						   _global_pos.lat, _global_pos.lon, _global_pos.alt,
+						   &dist_xy, &dist_z);
+
+			// close to waypoint, but altitude error greater than twice acceptance
+			if ((dist >= 0.0f)
+			    && (dist_z > 2.0f * _parameters.climbout_diff)
+			    && (dist_xy < 2.0f * acc_rad)) {
+
+				/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
+				position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+			}
+		}
+
+
+		if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 			_att_sp.thrust = 0.0f;
 			_att_sp.roll_body = 0.0f;
 			_att_sp.pitch_body = 0.0f;
 
-		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+		} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+
+			float position_sp_alt = pos_sp_curr.alt;
+
+			// Altitude first order hold (FOH)
+			if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
+			    ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
+			     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
+			     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
+			   ) {
+				const float d_curr_prev = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon,
+							  pos_sp_prev.lat, pos_sp_prev.lon);
+
+				// Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one
+				if (d_curr_prev > acc_rad) {
+					// Calculate distance to current waypoint
+					const float d_curr = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon,
+							     (double)curr_pos(0), (double)curr_pos(1));
+
+					/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
+					 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
+					_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
+
+					/* if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
+					 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
+					if (_min_current_sp_distance_xy > acc_rad) {
+						// The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
+						// radius around the current waypoint
+						const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt);
+						const float grad = -delta_alt / (d_curr_prev - acc_rad);
+						const float a = pos_sp_prev.alt - grad * _min_current_sp_distance_xy;
+
+						position_sp_alt = a + grad * _min_current_sp_distance_xy;
+					}
+				}
+			}
+
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			tecs_update_pitch_throttle(pos_sp_curr.alt,
+			tecs_update_pitch_throttle(position_sp_alt,
 						   calculate_target_airspeed(mission_airspeed),
 						   radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad,
 						   radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad,
@@ -936,7 +1010,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 						   false,
 						   radians(_parameters.pitch_limit_min));
 
-		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+		} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
 			_l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius,
@@ -984,10 +1058,10 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 						   false,
 						   radians(_parameters.pitch_limit_min));
 
-		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 			control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 
-		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+		} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 			control_takeoff(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 		}
 
@@ -1021,7 +1095,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
-			_att_sp.yaw_body = 0;
+			_att_sp.yaw_body = 0.0f;
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1114,7 +1188,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
-			_att_sp.yaw_body = 0;
+			_att_sp.yaw_body = 0.0f;
 		}
 
 	} else if (_control_mode.flag_control_altitude_enabled) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -193,6 +193,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 */
 
+	float	_min_current_sp_distance_xy{FLT_MAX};
+
 	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 */
 

src/modules/navigator/mission.cpp

@@ -54,6 +54,7 @@
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
 #include <navigator/navigation.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -228,10 +229,6 @@ Mission::on_active()
 			set_mission_items();
 		}
 
-	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
-
-		altitude_sp_foh_update();
-
 	} else {
 		/* if waypoint position reached allow loiter on the setpoint */
 		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -444,13 +441,12 @@ Mission::landing()
 void
 Mission::update_offboard_mission()
 {
-
 	bool failed = true;
 
 	/* reset triplets */
 	_navigator->reset_triplets();
 
-	struct mission_s old_offboard_mission = _offboard_mission;
+	mission_s old_offboard_mission = _offboard_mission;
 
 	if (orb_copy(ORB_ID(mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 		/* determine current index */
@@ -571,9 +567,6 @@ Mission::advance_mission()
 void
 Mission::set_mission_items()
 {
-	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
-	_min_current_sp_distance_xy = FLT_MAX;
-
 	/* the home dist check provides user feedback, so we initialize it to this */
 	bool user_feedback_done = false;
 
@@ -1048,14 +1041,6 @@ Mission::set_mission_items()
 		pos_sp_triplet->next.valid = false;
 	}
 
-	/* Save the distance between the current sp and the previous one */
-	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
-
-		_distance_current_previous = get_distance_to_next_waypoint(
-						     pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
-						     pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon);
-	}
-
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1240,81 +1225,6 @@ Mission::heading_sp_update()
 	}
 }
 
-void
-Mission::altitude_sp_foh_update()
-{
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	/* Don't change setpoint if last and current waypoint are not valid
-	 * or if the previous altitude isn't from a position or loiter setpoint or
-	 * if rotary wing since that is handled in the mc_pos_control
-	 */
-
-
-	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt)
-	    || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
-		 pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
-	    _navigator->get_vstatus()->is_rotary_wing) {
-
-		return;
-	}
-
-	// Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore
-	float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
-
-	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-		acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f);
-	}
-
-	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
-	if (_distance_current_previous - acc_rad < FLT_EPSILON) {
-		return;
-	}
-
-	/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
-	 * and the FW controller has a custom landing logic
-	 *
-	 * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
-	 * */
-	if (_mission_item.nav_cmd == NAV_CMD_LAND
-	    || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-	    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF
-	    || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
-	    || !_navigator->is_planned_mission()) {
-
-		return;
-	}
-
-	/* Calculate distance to current waypoint */
-	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-	/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
-	 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
-	_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
-						_distance_current_previous);
-
-	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
-	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
-	if (_min_current_sp_distance_xy < acc_rad) {
-		pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
-
-	} else {
-		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
-		 * of the mission item as it is used to check if the mission item is reached
-		 * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
-		 * radius around the current waypoint
-		 **/
-		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
-		float grad = -delta_alt / (_distance_current_previous - acc_rad);
-		float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
-		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
-	}
-
-	// we set altitude directly so we can run this in parallel to the heading update
-	_navigator->set_position_setpoint_triplet_updated();
-}
-
 void
 Mission::cruising_speed_sp_update()
 {

src/modules/navigator/mission.h

@@ -159,11 +159,6 @@ class Mission : public MissionBlock, public ModuleParams
 	 */
 	void heading_sp_update();
 
-	/**
-	 * Updates the altitude sp to follow a foh
-	 */
-	void altitude_sp_foh_update();
-
 	/**
 	 * Update the cruising speed setpoint.
 	 */
@@ -189,7 +184,7 @@ class Mission : public MissionBlock, public ModuleParams
 	 *
 	 * @return true if successful
 	 */
-	bool read_mission_item(int offset, struct mission_item_s *mission_item);
+	bool read_mission_item(int offset, mission_item_s *mission_item);
 
 	/**
 	 * Save current offboard mission state to dataman
@@ -246,7 +241,6 @@ class Mission : public MissionBlock, public ModuleParams
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_dist_between_wps,
-		(ParamInt<px4::params::MIS_ALTMODE>) _param_altmode,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mnt_yaw_ctl
 	)
 
@@ -271,11 +265,6 @@ class Mission : public MissionBlock, public ModuleParams
 	bool _mission_waypoints_changed{false};
 	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
 
-	float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint  */
-
-	float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet,
-					    only use if current and previous are valid */
-
 	enum work_item_type {
 		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */