FW Position Controller: output idle motor setpoint if landed and not in takeoff

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/PositionSetpoint.msg

@@ -7,7 +7,6 @@ uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
 uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
 uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
-uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
 
 uint8 LOITER_TYPE_ORBIT=0 	# Circular pattern
 uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -108,13 +108,6 @@ bool FlightTaskAuto::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
-	// The only time a thrust set-point is sent out is during
-	// idle. Hence, reset thrust set-point to NAN in case the
-	// vehicle exits idle.
-	if (_type_previous == WaypointType::idle) {
-		_acceleration_setpoint.setNaN();
-	}
-
 	// during mission and reposition, raise the landing gears but only
 	// if altitude is high enough
 	if (_highEnoughForLandingGear()) {
@@ -122,12 +115,6 @@ bool FlightTaskAuto::update()
 	}
 
 	switch (_type) {
-	case WaypointType::idle:
-		// Send zero thrust setpoint
-		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
-		_velocity_setpoint.setNaN();
-		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-		break;
 
 	case WaypointType::land:
 		_prepareLandSetpoints();

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -70,8 +70,7 @@ enum class WaypointType : int {
 	velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
 	loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
 	takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
-	land = position_setpoint_s::SETPOINT_TYPE_LAND,
-	idle = position_setpoint_s::SETPOINT_TYPE_IDLE
+	land = position_setpoint_s::SETPOINT_TYPE_LAND
 };
 
 enum class State {
@@ -135,7 +134,7 @@ protected:
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
 	bool _next_was_valid{false};
 	float _mc_cruise_speed{NAN}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
-	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
+	WaypointType _type{WaypointType::position}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
@@ -156,7 +155,7 @@ protected:
 	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;
-	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
+	WaypointType _type_previous{WaypointType::position}; /**< Previous type of current target triplet. */
 	bool _is_emergency_braking_active{false};
 	bool _want_takeoff{false};
 

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -706,11 +706,8 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 		}
 
 	} else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-		   && (_position_setpoint_current_valid
-		       || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+		   && _position_setpoint_current_valid) {
 
-		// Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
-		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
 		const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;
 
 		if (doing_backtransition) {
@@ -881,16 +878,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 	}
 
 	switch (position_sp_type) {
-	case position_setpoint_s::SETPOINT_TYPE_IDLE: {
-			_att_sp.thrust_body[0] = 0.0f;
-			const float roll_body = 0.0f;
-			const float pitch_body = radians(_param_fw_psp_off.get());
-			const float yaw_body = 0.0f;
-
-			const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body));
-			setpoint.copyTo(_att_sp.q_d);
-			break;
-		}
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
@@ -926,15 +913,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 
 #endif // CONFIG_FIGURE_OF_EIGHT
 
-	/* Copy thrust output for publication, handle special cases */
-	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-
-		_att_sp.thrust_body[0] = 0.0f;
-
-	} else {
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
-	}
+	_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
 
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(current_sp);
@@ -965,7 +944,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
 
 	} else {
-		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
+		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
 	}
 
 	const float pitch_body = get_tecs_pitch();
@@ -1463,7 +1442,7 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				   _param_climbrate_target.get(),
 				   is_low_height);
 
-	_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
+	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : min(get_tecs_thrust(), tecs_fw_thr_max);
 	const float pitch_body = get_tecs_pitch();
 
 	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
@@ -2111,7 +2090,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) *
 					       _flare_states.initial_throttle_setpoint;
 
-		_att_sp.thrust_body[0] = blended_throttle;
+		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : blended_throttle;
 
 	} else {
 

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -248,8 +248,7 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item,
 	}
 
 	// check if we find unsupported items and reject mission if so
-	if (mission_item.nav_cmd != NAV_CMD_IDLE &&
-	    mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
+	if (mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
 	    mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
 	    mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
 	    mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
@@ -346,8 +345,7 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item)
 	}
 
 	if (!_found_item_with_position) {
-		_found_item_with_position = (mission_item.nav_cmd != NAV_CMD_IDLE &&
-					     mission_item.nav_cmd != NAV_CMD_DELAY &&
+		_found_item_with_position = (mission_item.nav_cmd != NAV_CMD_DELAY &&
 					     mission_item.nav_cmd != NAV_CMD_DO_JUMP &&
 					     mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 					     mission_item.nav_cmd != NAV_CMD_DO_SET_HOME &&

src/modules/navigator/land.cpp

@@ -97,7 +97,6 @@ Land::on_active()
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 		_navigator->mode_completed(getNavigatorStateId());
-		set_idle_item(&_mission_item);
 
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

src/modules/navigator/loiter.cpp

@@ -76,45 +76,28 @@ Loiter::on_active()
 void
 Loiter::set_loiter_position()
 {
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
-	    _navigator->get_land_detected()->landed) {
-
-		// Not setting loiter position if disarmed and landed, instead mark the current
-		// setpoint as invalid and idle (both, just to be sure).
-
-		_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-		_navigator->set_position_setpoint_triplet_updated();
-		return;
-
-	}
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	if (_navigator->get_land_detected()->landed) {
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
+	// Check if we already loiter on a circle and are on the loiter pattern.
+	bool on_loiter{false};
+
+	if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
+	    && pos_sp_triplet->current.loiter_pattern == position_setpoint_s::LOITER_TYPE_ORBIT) {
+		const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
+					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+		on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);
+
+	}
+
+	if (on_loiter) {
+		setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
 
 	} else {
-		// Check if we already loiter on a circle and are on the loiter pattern.
-		bool on_loiter{false};
-
-		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
-		    && pos_sp_triplet->current.loiter_pattern == position_setpoint_s::LOITER_TYPE_ORBIT) {
-			const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
-						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-			on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);
-
-		}
-
-		if (on_loiter) {
-			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-		} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
-
-		} else {
-			setLoiterItemFromCurrentPosition(&_mission_item);
-		}
-
+		setLoiterItemFromCurrentPosition(&_mission_item);
 	}
 
 	// convert mission item to current setpoint

src/modules/navigator/mission_base.cpp

@@ -535,10 +535,8 @@ void MissionBase::setEndOfMissionItems()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	if (_land_detected_sub.get().landed) {
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-	} else {
+	// set a loiter item if not landed
+	if (!_land_detected_sub.get().landed) {
 		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
 

src/modules/navigator/mission_block.cpp

@@ -102,7 +102,6 @@ MissionBlock::is_mission_item_reached_or_completed()
 	case NAV_CMD_VTOL_LAND:
 		return _navigator->get_land_detected()->landed;
 
-	case NAV_CMD_IDLE: /* fall through */
 	case NAV_CMD_LOITER_UNLIMITED:
 		return false;
 
@@ -685,9 +684,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON);
 
 	switch (item.nav_cmd) {
-	case NAV_CMD_IDLE:
-		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-		break;
 
 	case NAV_CMD_TAKEOFF:
 	case NAV_CMD_VTOL_TAKEOFF:
@@ -844,22 +840,6 @@ MissionBlock::set_land_item(struct mission_item_s *item)
 	item->origin = ORIGIN_ONBOARD;
 }
 
-void
-MissionBlock::set_idle_item(struct mission_item_s *item)
-{
-	item->nav_cmd = NAV_CMD_IDLE;
-	item->lat = _navigator->get_home_position()->lat;
-	item->lon = _navigator->get_home_position()->lon;
-	item->altitude_is_relative = false;
-	item->altitude = _navigator->get_home_position()->alt;
-	item->yaw = NAN;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = true;
-	item->origin = ORIGIN_ONBOARD;
-}
-
 void
 MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode)
 {
@@ -1030,7 +1010,6 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i
 
 	if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND
 	    && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION
-	    && _mission_item.nav_cmd != NAV_CMD_IDLE
 	    && _navigator->get_local_position()->dist_bottom_valid
 	    && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param()
 	    && _navigator->get_local_position()->vz > FLT_EPSILON

src/modules/navigator/mission_block.h

@@ -197,11 +197,6 @@ protected:
 	 */
 	void set_land_item(struct mission_item_s *item);
 
-	/**
-	 * Set idle mission item
-	 */
-	void set_idle_item(struct mission_item_s *item);
-
 	/**
 	 * Set vtol transition item
 	 */

src/modules/navigator/navigation.h

@@ -60,7 +60,6 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_IDLE = 0,
 	NAV_CMD_WAYPOINT = 16,
 	NAV_CMD_LOITER_UNLIMITED = 17,
 	NAV_CMD_LOITER_TIME_LIMIT = 19,

src/modules/navigator/navigator_main.cpp

@@ -1176,7 +1176,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.cruising_speed = get_cruising_speed();
 	sp.cruising_throttle = get_cruising_throttle();
 	sp.valid = false;
-	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+	sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	sp.loiter_direction_counter_clockwise = false;
 }
 

src/modules/navigator/rtl_direct.cpp

@@ -352,7 +352,6 @@ void RtlDirect::set_rtl_item()
 		}
 
 	case RTLState::IDLE: {
-			set_idle_item(&_mission_item);
 			_navigator->mode_completed(getNavigatorStateId());
 			break;
 		}

src/modules/navigator/takeoff.cpp

@@ -72,17 +72,11 @@ Takeoff::on_active()
 
 		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-		// set loiter item so position controllers stop doing takeoff logic
-		if (_navigator->get_land_detected()->landed) {
-			_mission_item.nav_cmd = NAV_CMD_IDLE;
+		if (pos_sp_triplet->current.valid) {
+			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
 
 		} else {
-			if (pos_sp_triplet->current.valid) {
-				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-			} else {
-				setLoiterItemFromCurrentPosition(&_mission_item);
-			}
+			setLoiterItemFromCurrentPosition(&_mission_item);
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);